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ABSTRACT 


This  thesis  develops  the  mathematical  relationships  necessary  to  implement 
alternating  tripod  gaits  on  the  hexapod  underwater  walking  machine,  AquaRobot.  Analysis 
of  documentation  and  application  of  Denavit-Hartenberg  kinematic  modeling  techniques 
determine  the  fundamental  vehicle  parameters.  Smooth  leg  motion  models  following 
elliptical  and  cycloidal  trajectories  are  devised.  Gait  planning  algorithms,  using  the 
elliptical  smooth  leg  motion  model,  are  developed  for  both  discrete  and  continuous  body 
motion.  Statically  stable,  alternating  tripod  gait  simulations  are  implemented  in  the 
programming  language.  A  stick  figure  graphics  display  allows  examination  and  testing  of 
the  gait  algorithms  prior  to  incorporation  in  follow-on  3D  graphics  simulations  or  in  real¬ 
time  operation. 
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L  INTRODUCTION 


Most  of  us  are  familiar  with  the  development  and  widespread  use  of  wheeled 
locomotion.  Prepared  surfaces,  in  the  form  of  roadways  and  railways,  provide  the  means 
for  wheeled  vehicles  to  efficiently  transport  people  and  property  over  long  distances. 
Wheeled  vehicles,  however,  are  significantly  less  efficient  over  irregular,  unprepared 
terrain.  Even  tracked  vehicles,  whose  tracks  are  simply  elongated  wheels,  have  difficulty 
traversing  irregular  terrain.  Wheeled  and  tracked  vehicles,  then,  require  special  terrain. 

Legged  locomotion,  on  the  other  hand,  offers  several  potential  advantages  over 
wheels  or  tracks  when  traversing  natural  terrain.  First,  and  foremost,  legs  allow  the 
flexibility  to  select  and  place  supporting  feet.  Flexible  foot  placement  provides 
opportunities  to  test  the  terrain  prior  to  attempting  vehicle  support  and  extends  vehicle 
mobility  beyond  prepared  surfaces.  Legged  vehicles  can  move  on  more  general  terrain. 

Another  advantage  is  enhanced  traction  in  pliable  soil.  Wheeled  and  tracked  vehicles, 
as  an  inherent  part  of  their  design,  make  a  depression  in  any  non-supporting  surface  and 
then  constantly  try  to  climb  out  of  that  depression.  Legged  vehicles  typically  generate 
distinct  footprints  and  use  any  material  pushed  up  behind  the  foot  to  improve  traction. 
[MCG85] 

Additional  potential  advantages  of  legged  locomotion  over  wheeled  or  tracked 
locomotion  include  greater  speed,  less  power  consumption,  and  relatively  small  footprint. 
Finally,  adoption  of  legged  locomotion  offers  the  unique  advantage  of  integrating  the 
locomotion  function  with  a  terrain  measurement  function.  Specifically,  legged  locomotion 
allows  simultaneous  vehicle  support,  propulsion,  and  measurement  of  terrain  height  or 
depth. 


1 


Legged  locomotion  advantages  are  not  lost  in  an  underwater  environment.  Of  the 
vehicles  capable  of  submersible  operation,  90  percent  are  of  the  tethered  or  untethered 
free  swimming  type.  Although  these  vehicles  effectively  use  their  three-dimensional 
maneuverability,  they  have  difficulty  generating  significant  forces  and  have  trouble 
maintaining  stationary  position  and  direction.  The  underwater  wheeled  or  tracked 
(crawling)  vehicles  have  poor  maneuverability,  but  are  capable  of  exerting  very  large 
working  forces.  As  in  the  non-aquatic  case,  underwater  wheeled  or  tracked  locomotion  is 
limited  to  relatively  hard  and  flat  sea  bottom.  Additionally,  wheeled  or  crawling  vehicles 
make  the  water  so  muddy  that  optical  viewing  devices  or  television  cameras  are  rendered 
useless.  Even  in  an  underwater  environment,  legged  locomotion  overcomes  many  of  the 
disadvantages  of  wheeled,  tracked,  or  free  swimming  type  locomotion.  Legged  vehicles 
can  walk  on  uneven  terrain  with  minimal  disturbance  of  the  surface,  can  provide  a 
maneuverable,  yet  stable,  platform  for  observation  purposes,  and  can  use  appendages  for 
accurate  measurement.  [ISH83] 

A.  AQUAROBOT  UNDERWATER  WALKmG  MACHINE 

In  the  early  1980's,  the  Japanese  Port  and  Harbor  Research  Institute  (PHRI)  was 
tasked  with  finding  an  alternative  method  of  carrying  out  inspection  of  underwater 
construction  sites.  They  determined  a  robot  was  needed  because  of  increased  risks  to  and 
lower  working  efficiency  of  port  construction  workers  at  deeper  sea  areas  and  also 
because  of  a  shortage  of  divers.  Additionally,  the  PHRI  decided  the  ten  centimeter 
horizontal  and  five  centimeter  vertical  accuracy  requirements  and  average  working  depth 
of  50  meters  were  best  achieved  by  mechanical  means. 

TheAquaRobot  project  started  in  1984.  AquaRobot  was  originally  designed  with  two 
primary  functions:  1)  measurement  of  the  flatness  of  rock  foundation  mounds  for  tsunami 
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breakwaters  using  the  motion  of  its  six  walking  legs,  and  2)  observation  and  supervision  of 
underwater  construction  using  its  on-board  television  camera.  An  experimental  model  and 
a  prototype  model  were  constructed.  The  experimental  model  was  developed  for  above 
water  research  and  testing.  Based  on  the  results  of  testing  the  experimental  model,  a 
watertight  prototype  model  was  subsequently  developed. 

AquaRobot  (Figure  1-1)  is  a  six-legged,  “insect  type"  robot.  The  body  is  hexagonal 
and  the  legs  are  installed  on  the  sides  of  the  hexagonal  frame.  Each  of  its  six  legs  has  three 
articulated  joints  driven  by  DC  motors.  Each  leg  includes  a  touch  sensor  on  the  foot. 
AquaRobot  is  currently  controlled  by  a  16  bit  80286-based  microcomputer.  Additional 
geometrical  detail  of  AquaRobot  is  provided  in  Chapter  III.  A  more  complete  overview 
and  history  is  found  in  Appendix  A.  [AKI89] 

B.  MOTIVATION  FOR  AQUAROBOT  GAFT  PLANNING 

This  thesis  is  a  direct  outgrowth  of  a  cooperative  research  effort  between  the  Naval 
Postgraduate  School  (NFS)  and  the  Japanese  PHRI  to  enhance  the  hardware  and  software 
capabilities  of  the  AquaRobot  underwater  walking  robot.  The  National  Science 
Foundation  (NSF)  is  providing  support  to  the  NPS,  while  the  Japanese  Science  and 
Technology  Agency  (STA)  is  supporting  the  PHRI. 

The  PHRI  originally  developed  AquaRobot  to  replace  hard-hat  divers  constructing 
tsunami  barriers  in  the  hazardous  underwater  environment  off  Japan's  coast.  Although 
AquaRobot  has  demonstrated  significant  ability  as  the  first  hexapod  underwater  walking 
machine,  it  is  still  unable  to  perform  its  designed  task  more  efficiently  or  less  costly  than 
human  divers.  AquaRobot  speed  and  agility  enhancements  derived  from  gait  planning 
algorithms  in  this  thesis  Avill  result  in  a  more  efficient  and  less  costly  machine.  This 
translates  directly  into  reduced  risk  of  human  injury  and  decreased  construction  costs. 
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Figure  1-1.  The  Hexapod  Underwater  Walking  Machine  AquaRobot. 
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Although  walking  machine  research  and  construction  is  extensive,  AquaRobot  is  the 
first  walking  robot  with  a  practical  use.  As  such,  improvements  to  AquaRobot  directly 
result  in  concept  fulfillment  and  favorably  demonstrate  the  usefulness  of  robots,  and 
walking  robots  in  particular. 

C.  LITERATURE  SURVEY 

Obviously,  the  most  reviewed  literature  was  that  generated  at  the  PHRI  by  Akizono 
[AKI89]  and  Iwasaki  [IWA87],  [IWA88a],  [IWA88b],  and  [IWA90].  These  papers 
provided  AquaRobot-sp&o&c  development  and  implementation  information.  Craig's 
textbook  [CRA86]  yielded  the  fundamental  robotics  techniques  for  solutions  of 
kinematics,  inverse  kinematics,  coordinate  transformations,  and  workspace  problems. 

Some  information  found  in  Hirose's  seminal  series  of  papers  on  gait  algorithms  for 
quadruped  vehicles  [HIR84],  [HIR86a],  [HIR86b],  and  [HIR88]  proved  transferable  to 
hexapod  vehicles.  Two  of  McGee's  papers  [MCG85],  [MCG79]  were  consulted  for  their 
insight  into  hexapod  walking  issues. 

Although  several  walking  robot  textbooks  were  reviewed,  specific  walking  theory 
was  best  presented  in  Song's  textbook  [SON89],  while  general  walking  theory  was 
covered  quite  well  in  Toad'-'  textbook  [TOD853. 

Lyman's  thesis  [LYM87],  along  with  papers  by  Kwak  [KWA90],  Lee  [LEE88a]  and 
[LEE88b],  and  Waldron  [WAL84]  pro'/ided  analysis  of  gait  control  for  the  Adaptive 
Suspension  Vehicle,  a  hexapod  walking  machine  vwth  bilateral  symmetry. 

Davidson's  [DAV93]  and  Grim's  [GRI93]  theses  provided  baseline  graphics  code 
development  information.  Kanayama's  manuscript  on  spatial  reasoning  fKAN92]  proved 
the  best  source  for  efficient  motion  planning  algorithms. 
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D.  THESIS  ORGANIZATION 


The  final  part  of  this  chapter  is  devoted  to  outlining  the  thesis  organization.  Chapter 
II  provides  the  setting  for  the  remainder  of  the  thesis.  A  brief  history  of  walking  machines 
is  presented,  along  with  a  survey  of  previous  work  and  an  introduction  to  walking  theory. 

A  physical  description  of  the  AquaRobot  underwater  walking  robot  is  the  emphasis  of 
Chapter  III.  Geometrical  features  and  system  parameters  are  defined.  Some  aspects  of 
AquaRobot  are  idealized  for  simplification.  A  detailed  problem  statement  is  included  in 
this  chapter. 

Existing  AquaRobot  documentation  is  sparse  and  most  detailed  information  is  written 
in  Japanese.  Therefore,  Chapter  IV  develops  fiindamental  vehicle  parameters  through 
analysis  of  written  reports,  review  of  construction  blueprints,  and  application  of  Denavit- 
Hartenberg  kinematic  modeling  techniques.  Inverse  kinematic  equations  and  relations  are 
presented  and  necessary  coordinate  transformations  are  developed.  Workspace  volumes 
are  determined,  appropriate  workspace  constraints  identified,  and  the  strategy  for 
workspace  use  is  presented. 

Chapter  V  introduces  the  stability  model  used  throughout  this  thesis.  AquaRobot's 
center-of-body  and  center-of-gravity  relations  are  defined.  The  issue  of  static  versus 
dynamic  stability  is  addressed. 

AquaRobot  currently  uses  rectangular  leg  motions  for  all  gaits.  Curved  leg  motions 
result  in  smoother  gaits.  In  Chapter  VI,  two  smooth  leg  motion  models  are  developed. 
Elliptical  and  cycloidal  leg  motion  curves  are  generated  for  use  in  the  AquaRobot 
simulator. 

Statically  stable  tripod  gait  are  planned  and  implemented  in  Chapter  VII.  To  ease 
initial  gait  implementation,  flat  terrain,  fixed  body  height,  fixed  body  orientation,  and 
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straight  line  path  constraints  are  placed  on  the  vehicle.  Some  constraints  are  lifted  as  the 
thesis  progresses.  Ultimately,  a  continuous  omnidirectional  tripod  gait  is  developed. 

Chapter  VIII  presents  the  AquaRobot  simulation  program.  The  simulation  features 
and  program  organization  are  described.  The  final  chapter  summarizes  the  contributions  of 
this  thesis.  Future  research  areas  are  also  outlined.  Appendix  A  contains  an  overview  and 
history  of  the  AquaRobot.  Appendices  B  and  C  contain  program  code.  Appendix  D  is  an 
AquaRobot  Simulator  User's  Manual  that  is  separable  fi'om  the  thesis. 


7 


n.  SURVEY  OF  PREVIOUS  WORK 


Three  aspects  of  practical  walking  machine  design  are  considered  crucial;  control  of 
legged  vehicles,  actuator  and  leg  design,  and  gait  study.  Vehicle  control  is  considered  the 
most  crucial  because  of  its  inherent  complexity.  Many  researchers  are  currently  devoting 
extensive  efforts  to  this  research  area.  Only  a  few  walking  machines  have  sophisticated  leg 
designs.  Serious  study  has  only  recently  been  given  to  improving  leg  efficiency  through 
better  designs  of  legs  and  actuators.  Finally,  previous  gait  study  has  been  concentrated  on 
straight  line  motion  over  flat  terrain.  This  thesis  provides  the  foundation  to  extend  gait 
study  'mAquaRobot  beyond  these  motion  and  terrain  constraints.  [TOD8S] 

A  A  BRIEF  HISTORY  OF  WALKING  MACHINES 

Walking  machines  have  been  constructed  with  from  one  to  eight  legs,  and  possibly 
more.  An  even  number  of  legs  is  almost  universal  as  this  allows  efficient  gaits  for 
progression  in  a  straight  line.  More  legs  were  typically  used  in  the  past  for  heavily  loaded, 
but  slower  vehicles,  while  bipeds  and  quadrupeds  were  generally  faster  and  more  agile.  Sue 
legs  is  a  magic  number  because  it  allows  two  alternating  tripods.  Two  legs  is  another 
popular  number  because  it  allows  emulation  of  human  walking.  Some  of  the  many 
properties  to  consider  when  selecting  the  number  of  legs  are:  1)  stability,  2)  energetic 
efficiency,  3)  redundancy  (using  fewer  legs  to  walk  if  some  are  unavailable),  4)  joint 
control  requirements,  5)  cost,  6)  weight,  7)  desired  complexity  of  sensing,  and  8)  possible 
gaits.  [TOD85] 
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Two  of  the  earliest  examples  of  inventors'  attempts  to  design  walking  machines  are 
Rygg's  mechanical  horse  in  1893  and  Bechtolsheim's  four-legged  machine  in  1913  There 
is  no  indication  that  either  machine  actually  walked.  [SON89] 

In  the  mid  1950’s,  numerous  research  groups  began  to  systematically  study  walking 
machines  [SON89].  Then,  in  the  1960's,  the  Space  General  Corporation  designed  and 
constructed  a  six-legged  machine  and  an  eight-legged  machine  to  investigate  their 
applicability  as  lunar  explorers.  These  two  machines  met  their  design  goals,  but  exhibited 
poor  terrain  adaptability  because  of  too  few  degrees  of  freedom.  [TOD85] 

In  1966,  Frank  and  McGhee  developed  the  first  autonomous  legged  vehicle  to  walk 
under  computer  control.  They  called  their  walking  machine  the  "Phoney  Pony".  This  four- 
legged  machine  was  powered  externally  via  a  cable  and  only  walked  in  a  straight  line.  In 
1968,  the  General  Electric  Corporation  built  another  quadruped,  a  3000  pound  vehicle 
with  12  degrees  of  freedom  and  a  human  rider-operator.  The  General  Electric  quadruped 
was  successful  at  obstacle  climbing  and  possessed  good  mobility  in  difficult  terrain. 
Because  its  operation  was  highly  complex  and  demanding,  however,  only  a  few  people 
ever  learned  how  to  operate  it.  The  General  Electric  quadruped  proved  the  necessity  for 
computer  control  of  multi-degree-of-freedom  vehicles.  [TOD85] 

In  the  1970's,  several  walking  machines  were  constructed.  A  pneumatically  powered 
and  analog  computer  controlled  biped  was  successfully  operated  in  Yugoslavia.  A  series 
of  bipeds  was  developed  in  Japan,  with  walking  speeds  ranging  from  90  seconds  per  step 
to  the  speed  of  a  slow  human  walk.  From  1974  through  1979,  Russia  developed  and 
operated  two  six-legged  walking  machines  of  the  insect  body  type.  Finally,  in  1977, 
McGhee  and  his  associates  at  the  Ohio  State  University  (OSU)  first  got  the  OSU  Hexapod 
to  walk.  [TOD85] 
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With  the  progress  in  microcomputer  technology,  computer-aided  design,  and 
computer  simulation,  the  1980's  ushered  in  the  development  of  several  new  walking 
machines.  In  1980,  Hirose  and  Umetani,  at  the  Tokyo  Institute  of  Technology,  built  the 
four-legged  Perambulating  Vehicle  II  (PVII).  The  PVII  was  promising  because  of  its 
sophisticated  pantograph  leg  design.  Hirose,  in  1984,  constructed  an  enlarged  version  of 
the  PVII,  called  TITAN  III.  In  1982,  the  Ohio  State  University  developed  monopod  and 
six-legged  walking  machines  as  part  of  the  Adaptive  Suspension  Vehicle  (ASV)  project. 
[TOD85] 

More  recently.  Brooks,  at  the  Massachusetts  Institute  of  Technology,  has  designed 
and  built  several  insectoid  walking  robots,  one  named  Atilla,  to  demonstrate  his  theories 
on  subsumption.  Additionally,  the  Camaegie-Mellon  University  is  working  on  a  walking 
machine,  the  Ambler,  designed  to  explore  the  surface  of  Mars.  Finally,  research  continues 
on  AquaRobot  at  the  Naval  Postgraduate  School  and  the  Japanese  PHRI. 

B.  SURVEY  OF  HEXAPOD  VEHICLES 

Many  hexapod  vehicles  have  oeen  designed  and  built  to  advance  research  in  walking 
control,  leg  and  actuator  design,  and  gait  planning.  Six  legs  is  a  good  compromise 
between  speed  (shown  in  the  next  section),  stability  (generally  improved  \vith  a  larger 
number  of  legs),  and  simplicity  [WAL84].  The  incremental  stability  gain  is  smaller  when 
going  from  six  legs  to  eight  legs  than  when  going  from  four  legs  to  six  legs  [WAL84].  So, 
although  there  is  a  marked  advantage  to  using  hexapod  vehicles  over  quadruped  vehicles, 
the  increasing  mechanical  and  computational  complexity  of  eight-legged  vehicles  offsets 
their  minimal  stability  advantages. 

One  of  the  first  successful  hexapod  walking  machines  was  that  constructed  at  the 
University  of  Rome  in  1972.  This  machine  was  simply  a  six-legged  version  of  the  Phoney 
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Pony.  Then,  as  mentioned  in  the  previous  section,  the  OSU  Hexapod  walked  in  1977  In 
1983,  Odetics,  Incorporated  constructed  the  ODEX  I,  a  six-legged  walking  machine  with 
some  climbing  ability.  ODEX  II,  also  a  hexapod  machine,  has  since  been  designed  for  use 
in  nuclear  power  plants  [ODE86].  In  1985,  the  design  and  construction  of  a  large,  man¬ 
carrying  hexapod  called  the  Adaptive  Suspension  Vehicle  was  completed  at  OSU 
[MCG86].  The  ASV  is  a  self-contained  walking  machine  designed  to  traverse  natural 
terrain.  Each  of  the  hexapods  discussed  has  made  a  great  contribution  to  the  study  of 
walking  control,  leg  and  actuator  design,  and  gait  planning.  However,  of  all  the  known 
hexapods,  only  AquaRobot  has  moved  beyond  the  research  phase  into  practical  use. 

C.  INTRODUCTION  TO  WALKING  THEORY 

The  basic  definitions  and  theorems  for  hexapod  tripod  gait  planning  used  in  this  paper 
are  introduced  in  this  section.  A  gait  is  a  method  of  locomotion  distinguished  by  a  specific 
pattern  of  lifting  and  placing  of  the  feet.  Gaits  are  often  described  using  McGhee’s  and 
Jain's  event  sequence  notation  [MCG72].  In  such  a  notation,  the  integer  /  corresponds  to 
the  event  of  placing  foot  i  on  the  ground.  Integer  i+n,  where  n  equals  the  number  of  legs, 
represents  lifting  the  same  foot.  AquaRobot's  legs  are  numbered  sequentially  in  a 
clockwise  direction  from  one  t'nrough  six. 

The  transfer  phase  of  a  leg  is  that  time  period  when  the  foot  is  not  touching  the 
ground.  The  transfer  phase  is  also  known  as  the  recovery  time  t  or  the  return  stroke 
[TOD85].  The  support  phase  L  of  a  leg,  then,  is  simply  the  opposite  of  the  transfer  phase, 
the  time  period  when  the  foot  is  touching  the  ground.  The  cycle  time  T  is  the  time  required 
for  a  complete  cycle  of  leg  locomotion  of  a  periodic  gait.  The  cycle  time  includes  the 
transfer  and  support  phases  of  the  leg.  A  periodic  gait  is  one  in  which  every  limb  operates 
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with  the  same  cycle  time.  Otherwise,  the  gait  is  non-periodic  The  duty  factor  is  the 
fraction  of  the  cycle  time  in  which  leg  i  is  in  the  support  phase.  [SON89]  Thus, 


time  of  support  phase  of  leg  i  _  t^ 
cycle  time  of  leg  i  f 


(2.1) 


A  periodic  gait  is  singular  if  there  is  a  simultaneous  occurrence  of  two  or  more 
events  during  a  locomotion  cycle.  Conversely,  a  periodic  gait  is  nonsingular  if  no  two  of 
its  events  occur  simultaneously  [MCG68a].  A  gait  is  considered  regular  if  all  legs  have  the 
same  duty  factor,  as  when 


U.j^l,2,...,n 
[w  is  the  leg  number 


(2.2) 


The  leg  stride  is  a  complete  cycle  of  leg  movements  from  a  particular  leg  movement 
to  the  next  occurrence  of  the  same  leg  movement  [TOD85].  The  stride  frequency  /is  the 
number  of  strides  in  unit  time  [TOD85].  The  stride  length  X  is  the  distance  the  center  of 
gravity  of  the  body  travels  during  one  stride  [TOD85].  The  leg  stroke  R  is  the  distance  the 
foot  travels,  relative  to  the  body,  during  the  support  phase  [SON89].  A  step  is  defined  as 
the  interval  from  the  time  a  leg  is  placed  until  the  time  the  next  leg  is  placed  [HIR84]. 

A  working  volume  is  associated  with  each  leg.  This  volume  is  a  subset  of  the  three- 
dimensional  space,  defined  relative  to  the  body,  consisting  of  the  points  which  the  foot  can 
reach  [KWA90].  The  support  pattern  of  a  walking  machine  is  defined  as  "the  two 
dimensional  point  set  in  a  horizontal  plane  consisting  of  the  convex  hull  of  the  vertical 
projection  of  all  foot  points  in  the  support  phase"  [SON89].  Generally,  the  contact  point 
between  the  foot  and  the  ground  is  idealized  to  have  no  slip.  Further,  although  the  actual 
foot  contact  may  be  distributed  over  some  small  surface  area,  the  center  of  pressure  may 
be  chosen  as  the  foot  contact  point.  A  foothold  is  described  as  a  point  on  a  piece  of  terrain 
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[KWA90].  When  a  foot  is  placed  on  the  terrain,  its  foothold  becomes  the  support  point  of 
the  leg  [KVVA90].  A  foothold  can  be  assigned  to  a  leg  while  it  is  still  in  the  transfer  phase 

There  are  four  important  definitions  related  to  stability.  First,  stability  margin  for 
an  arbitrary  support  pattern,  is  the  shortest  distance  from  the  vertical  projection  of  the 
center  of  gravity  to  any  point  on  the  boundary  of  the  support  pattern  in  the  horizontal 
plane  [KWA90].  Then,  the  front  stability  margin  and  the  rear  stability  margin  describe 
distances  from  the  vertical  projection  of  the  center  of  gravity  to  the  forward  and  rearward 
boundaries  of  the  support  pattern,  respectively  [SON89].  The  front  and  rear  stability 
margins  are  measured  along  the  direction  of  motion.  Finally,  the  longitudinal  stability 
margin  S,  is  defined  as  the  shortest  distance  from  the  body’s  center  of  gravity  to  the 
boundary  of  the  support  pattern,  measured  in  the  direction  of  travel  [MCG86].  From  the 
definition  of  longitudinal  stability  margin,  a  gait  is  deemed  statically  stable  if  5^,  >  0 
Otherwise,  it  is  deemed  statically  unstable. 

With  some  fundamental  gait  definitions  explained,  we  can  now  show  that  vehicle 
speed  increases  as  the  number  of  legs  increases.  Waldron  et  al.  [WAL84]  and  Todd 
[TOD85]  agree  that  the  major  limitation  on  speed  of  legged  locomotion  is  the  time 
required  to  return  the  leg  through  the  air  to  its  starting  position.  For  any  legged  vehicle, 

there  is  a  minimum  time  for  a  leg  to  be  in  this  transfer  phase.  Additionally,  there  is  a 
minimum  duty  factor  for  statically  stable  systems;  quadrupeds, 

for  hexapods,  and  for  eight  legs.  Finally,  if  the  leg  stroke  R 

return  stroke  t  and  cycle  time  T  are  presumed  constant,  the  duty  factor  (3  causes  the  speed 
I '  to  change  as 


R 


From  previous  definitions, 


^  ^support  phase  ^ tranter  phase  ^s 


+  T. 


(2.4) 


Then,  using  equation  2. 1 

Combining  equations  2.4  and  2.5 

T=Tfi+T. 


Then,  for  a  fixed  t. 


T  = 


\-P 


Finally,  combining  equations  2.3  and  2.7  yields 

r(  1-A^ 


v= 


P 


Substituting  into  equation  2.8  yields 


3t 


Using  Phcuspod  equation  2.8  yields 


T 


Lastly,  using  Peight-ugs  equation  2.8  provides 


(2.5) 

(2.6) 

(2.7) 

(2.8) 

(2.9) 

(2.10) 

(2.11) 


From  these  relations,  there  is  obviously  a  speed  increase  as  the  number  of  legs 
increases  from  four  to  eight.  However,  as  previously  noted,  the  speed  increase  from  four 
legs  to  six  legs  is  threefold,  whereas  the  speed  increase  from  six  legs  to  eight  legs  is  only 
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two-thirds.  Herein  lies  one  reason  hexapod  walking  machines  are  so  prevalent  over  other 
walking  machines. 

D.  SUMMARY 

This  chapter  provides  background  and  introductory  information  helpful  for 
understanding  the  gait  planning  research  undertaken  in  this  study.  A  general  history  of 
walking  machines  is  followed  v  ith  a  more  specific  survey  of  hexapod  walking  machines. 
Terminology  and  methods  typical  of  the  gait  planning  problem  are  defined.  Other,  more 
specific,  definitions  are  presented  as  material  develops  throughout  the  thesis. 

The  following  chapter  contains  descriptions  of  AquaRobot,  the  model  used  to 
simulate  AquaRobot,  and  the  gait  chosen  for  development  in  this  thesis. 
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m.  ROBOT  DESCRIPTION  AND  SIMULATION  MODEL 


This  chapter  is  intended  as  a  description  of  the  geometrical  features  of  AquaRobot,  a 
description  of  the  AquaRobot  model  adopted  for  this  study,  and  a  discussion  of  the 
constraints  employed  to  limit  the  scope  of  this  study.  Additionally,  this  chapter  describes 
the  specific  gait  planning  and  simulation  issues  undertaken  in  this  thesis. 

A.  AQUAROBOT 

As  mentioned  previously,  AquaRobot  is  a  six-legged,  insect-type  robot.  The  body  is 
hexagonal  and  the  legs  are  installed  on  the  sides  of  the  hexagonal  frame.  Figure  3-1  shows 
the  major  dimensions  and  sensor  locations  of  AquaRobot.  In  this  figure,  the  camera 
manipulator  is  shown  positioned  between  legs  one  and  two,  with  leg  one  pointing  directly 
to  the  reader's  right. 

Figure  3-2  is  a  detailed  plan  view  of  AquaRobot.  In  this  figure,  the  individual  leg 
numbers  are  shown.  The  legs  are  placed  at  60°  increments  about  the  body.  This  figure  also 
includes  a  detail  of  the  underwater  TV  camera,  ultrasonic  ranging  device,  and  lights  fitted 
to  the  manipulator  boom  mounted  on  top  of  the  robot's  body.  The  manipulator  boom  has 
three  articulations  similar  to  those  found  in  the  legs.  The  first  articulation  revolves  about  a 
vertical  axis  through  the  centerline  of  the  robot's  body.  The  next  two  articulations  revolve 
about  horizontal  axes.  Altogether,  these  three  articulations  give  the  camera  three  degrees 
of  freedom. 

Figure  3-3  presents  a  detailed  side  view  of  AquaRobot.  Legs  one  and  four  are  shown 
with  dimensions  representative  of  all  six  legs.  The  manipulator  boom  is  shown  fully 
extended  with  its  maximum  reach  of  155  centimeters.  This  figure  also  shows  leg 
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articulations  representative  of  all  six  legs.  The  first  articulation  is  located  at  the 
intersection  of  the  legs  and  the  body.  This  articulation  rotates  about  a  vertical  axis  and  is 
also  referred  to  as  articulation  one,  joint  one,  the  azimuth  joint,  or  the  HIP,  depending  on 
the  reference.  The  typical  HIP  joint,  then,  is  located  37.5  centimeters  outboard  from  the 
center  of  the  body  at  0°,  60“,  120°,  180°,  240°,  and  300°  increments.  The  second 
articulation  is  20  centimeters  outboard  of  the  HEP.  It  rotates  about  a  horizontal  axis  and  is 
also  known  as  articulation  two,  joint  two,  the  elevation  joint,  or  KNEEL  The  third 
articulation,  also  known  as  articulation  three,  joint  three,  the  knee  joint,  or  KNEE2,  is  50 
lineal  centimeters  outboard  of  KNEEl  and  also  rotates  about  a  horizontal  axis.  The 
fourth,  and  final,  articulation  is  known  as  articulation  four  or  joint  four.  It  is  a  passive  ball- 
and-socket  joint  that  attaches  the  foot  to  the  leg  and  is  100  centimeters  from  KNEE2. 

Figure  3-4  is  a  detailed  view  representing  a  typical  leg's  structure.  In  this  figure,  joint 
rotational  configurations  are  shown.  For  the  HIP,  a  positive  angle  represents  a  clockwise 
(CW)  rotation  of  the  joint.  For  the  KNEEl  and  KNEE2  joints,  a  positive  angle  represents 
lifting  the  leg  segment.  Although  the  foot  is  not  actively  (motor)  controUed,  it  is  allowed 
to  rotate  passively  within  ±  45  degrees  about  joint  four.  Figure  3-4  also  shows  cutaway 
views  of  the  previously  mentioned  harmonic  gears,  bevel  gears,  DC  motors,  and  motor 
encoders. 

Table  3-1  provides  a  synopsis  some  of  AquaRobot's  more  relevant  characteristics. 
AquaRobot,  with  its  present  walking  and  control  algorithms,  can  achieve  a  maximum 
speed  of  7.75  meters/minute  when  walking  on  flat  surface  terrain  using  a  rectangular 
alternating  tripod  gait.  The  rectangular  terminology  refers  to  the  way  the  foot  is  moved:  1) 
first  the  foot  is  lifted  directly  upward,  2)  then  the  foot  is  moved  horizontally  in  the 
direction  of  travel  for  one  stride,  and  3)  finally  the  foot  is  lowered  directly  to  touch  the 
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Figure  3-1.  Major  Dimensions  and  Sensors  (After  [IWA90]). 
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Figure  3-4.  Leg  Structure  (After  [AKI89]). 


21 


FOOT 


Table  3-1.  ylgLM/JoBor Prototype  Characteristics  (after  [iwa901). 


VEfflCLE  TYPE 

axis-synunetric 

6-legged 

insect-type 

walking 

JOINT  TYPE 

4  joints  per  leg 

joint  1 :  active,  vertical  axis,  revolute 
joint  2:  active,  horizontal  axis,  revolute 
joint  3 ;  active,  horizontal  axis,  revolute 
joint  4:  passive,  ball-and-socket 

JOINT  DRIVE  METHOD 

semi-direct  drive,  DC  servo  motor 

CONTROL  METHOD 

80286-based  microcomputer 
controlling  hardware  via  software  algorithms 

MAXIMUM  WALKING  SPEED 

7.75  meters/minute  (flat  land) 

MAXIMUM  ROTATING  RATE 

445  degrees/minute  (flat  land) 

MAIN  MATERIAL  USED 

anti-corrosive  aluminum 

VEHICLE  WEIGHT 

SENSORS 

6  tactile  sensors 

2  inclinometers 

1  flux  gate  gyrocompass 

1  pressure  sensor 

MANIPULATOR 

CHARACTERISTICS 

TV  camera  end-effector  with 
ultrasonic  range  finding  capability 

MAXIMUM  TERRAIN  ROUGHNESS 

±35  centimeters 

MINIMUM  TERRAIN  ROUGHNESS 

±  5  centimeters 

WATERTIGHT  DEPTH 

50  meters 

TETHER  CHARACTERISTICS 

100  foot  length 

42  millimeter  diameter 

18  metal  wire  conductors 
optical  fiber  link 

1500  kgf  tensile  strength 

NAVIGATION 

man-machine  interface  (XYZ  inputs) 
dead  reckoning 
transponder  system 

PURPOSES 

measure  flatness  of  rubble  mound 
observe  underwater  structures 
supervise  underwater  construction 
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terrain.  The  foot  motion  trace,  or  imaginary  path  draAvn  in  the  air  (or  water)  by  the  foot, 
would  describe  three  sides  of  a  rectangle. 

AquaRobot  can  rotate  on  its  vertical  centerline  in  a  CW  or  counter  clockwise  (CCW) 
direction  if  desired.  The  maximum  rotating  rate  achieved  is  445  degrees/minute. 
AquaRobot  was  designed  to  walk  in  terrain  with  a  maximum  roughness  of  ±  35 
centimeters,  representative  of  a  typical  unleveled  rubble  mound.  A  typical  leveled  rubble 
mound  has  a  maximum  roughness  of  ±  5  centimeters. 

B.  SIMULATION  MODEL 

The  hexapod  vehicle  simulation  model  used  in  this  thesis  is  the  prototype  AquaRobot 
presently  under  test  at  the  Japanese  PHRI.  Figure  3-5  shows  plan  and  side  views  of  the 
simulation  model  of  AquaRobot  with  the  earth-  and  body-fixed  coordinate  systems.  In  this 
study,  the  earth-fixed  coordinate  system  is  known  as  the  world  while  the 

body-fixed  coordinate  system  is  known  as  the  body  iXg,yg,Zg).  The  body  coordinate 
system  is  fixed  at  the  center  of  the  plane  defined  by  the  HIPs  of  all  six  legs.  The  x^-axis 
lies  along  the  line  joining  the  HIPs  of  legs  one  and  four  and  passing  through  the  center  of 
the  body.  Positive  x  is  in  the  direction  of  leg  one.  The  j^-axis  is  perpendicular  to  the 
Xg-axis  at  the  center  of  the  body  and  passes  between  legs  two  and  three  and  legs  five  and 
six.  Positive  is  in  the  direction  between  legs  two  and  three.  The  z^-axis  lies  orthogonal 
to  the  Xg  and  yg  axes  with  positive  z  following  the  right-hand  rule  convention;  positive  Zg 
is  down. 

Figure  3-6  shows  the  relationship  between  the  physical  and  modeled  walking 
machine.  A  representative  side  view  that  includes  legs  one  and  four  is  shown.  Leg  four 
(LEG4)  is  a  scale  drawing  of  an  actual  AquaRobot  leg.  Leg  one  (LEGl)  is  depicted  in 
linkage  form.  Note  from  the  figure  that  the  joints  are  numbered  fi'om  inboard  outward. 
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J0INT3  YIY  joints 


LINKS 


Figure  3-6.  Relationship  Between  the  Physical  and  Modeled  Robot. 


with  joint  zero  (JOINTO)  located  at  the  robot's  center-of-body.  Links  are  numbered 
following  the  inboard  joint.  Therefore,  link  zero  (LINKO)  is  between  JOINTO  and  joint 
one  (JOINTl),  link  one  (LINKl)  is  between  JOINTl  and  joint  two  (JOINT2),  etc.  The 
action  of  joint  four  (JOINT4)  is  not  modeled  in  this  study.  Instead  of  modeling  JOINT4 
and  the  entire  foot  pad,  a  point  foot  is  used  where  the  junction  of  link  three  (LINKS) 
would  meet  JOINT4. 

At  the  outset  of  this  study,  AquaRobot  simulation  system  parameters  were  adopted  to 
ensure  come  consistency  in  program  development.  The  system  is  described  in  equations 
3.1  through  3.6.  Table  3-2  then  lists  the  variables  used  in  the  system  description  and  their 
meaning.  The  System  is  defined  as  a  state  vector  that  includes  the  states  of  the  legs  and 
body  X  and  the  warning  flags  F .  Then, 

=(x,f)  (3.1) 

and 

^  =  =  (3-2) 

The  leg  states  x,  are  further  subdivided  into  position,  velocity,  and  acceleration  vectors 

~  (^tl  , . . .  ,Fic6yPt6  ’Pa6  ) ’ 

~  (Ai  ^Pt\  "Pa\  >  •  "Pk6  »^o6  )• 

The  body  states  Xj,  are  then 

^6  (3.4) 

The  flag  vectors  F  provide  warnings  of  foot  contacts  with  the  terrain  and  of  joint 
limits  reached.  Therefore, 

P  =  (Fc^.Fu^)  =  (F^.Pl)  (3.5) 
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The  flag  vectors  are  also  subdivided  into  foot  contact  and  joint  limit  sub-vectors 


Fc  =(C,,...,Q),  and 


(3.6) 


These  system  state  vectors  allow  code  fi^om  several  authors  to  wori;  together  without 
significant  problems.  Each  author  may  still  refer  to  parameters  using  terminology  that  best 
describes  the  use  of  that  parameter.  For  example,  this  author  refers  to  joint  one  as  the 
HIP,  joint  two  as  KNEE  1 ,  and  joint  three  as  KNEE3 . 


Table  3-2.  Simulation  System  Variable  Descriptions. 


3 

Joint  Angular  Displacement 

k  k 

Knee  joint  (KNEE2),  legs  1  through  6 

Elevation  joint  (KNEEl),  legs  1  through  6 

Xp  or  x„ 

North 

ys  or 

East 

2e  or 

Down 

RoU 

0 

Elevation 

v|/ 

Azimuth 

1  " 

Body  velocity  with  respect  to  earth  in 

1  ^ 
w 

body  coordinates  (principal  axes). 

p 

r 

Body  angular  rate  in  body  coordinates. 

c  c 

Foot  contact  flag,  legs  1  through  6 

L 

Joint  limit  flag,  all  18  joints 

Many  simplifying  assumptions  were  made  within  this  model  of  AquaRobot.  The 
simplifications  were  made  primarily  to  speed  development  of  the  gait  planning  algorithms 
and  graphics  routines.  However,  each  program  was  devised  with  future  expansion  in  mind. 
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Whenever  possible,  program  code  was  modularized  for  future  expansion.  Specific 
simplifications  are  addressed  in  the  following  paragraphs. 

1.  Flat  Terrain 

The  first  simplifications  concern  the  vehicle's  operating  environment.  Although 
the  actual  AquaRobot  was  designed  to  operate  in  uneven  terrain,  the  simulation  model 
initially  developed  included  only  flat  terrain  with  no  obstacles.  The  flat  terrain  is 
graphically  displayed  using  a  checkerboard  design.  The  simulation  process  does  allow 
feedback  of  foot  contacts  with  the  flat  terrain.  Expansion  to  allow  foot  contact  feedback 
when  the  foot  encounters  unstructured  terrain  is  present. 

2.  Fixed  Body  Height 

Because  the  terrain  is  flat,  the  robot's  body  height  and  horizontal  body  angle  are 
fixed  arbitrarily  at  the  outset  of  the  program.  AquaRobot  has  both  selectable  body  height 
and  terrain  following  capability.  Neither  of  these  features  is  necessary  to  implement  a 
tripod  gait.  Selectable  body  height  is  a  feature  of  the  simulation.  Terrain  following 
capability  can  easily  be  added  as  a  code  module  if  future  research  warrants  its  use. 

3.  Straight  Line  Path 

Again,  for  ease  in  initial  program  development,  a  straight  line  path  oriented  with 
leg  one  following  the  positive  x^-axis  was  adopted.  When  turning  is  considered,  straight 
line  paths  between  two  points  is  still  used.  However,  the  robot  is  not  required  to  rotate  to 
have  LEGl  align  with  the  desired  direction  of  travel.  Some  smooth  path-following 
techniques  were  implemented  as  part  of  this  thesis. 


28 


4.  Constrained  Workspace 

Because  of  its  leg  design,  AquaRobot  has  the  ability  to  step  into  or  on  its  own 
feet  or  legs.  Although  this  feature  allows  much  greater  freedom  when  determining 
footholds,  it  is  not  beneficial  at  the  outset  of  program  design.  Therefore,  the  workspace  of 
AquaRobot  was  constrained  so  no  leg  or  foot  overlap  was  allowed.  Other  workspace 
constraints,  or  none  at  all,  are  possible  by  modifying  the  existing  program. 

5.  Dynamics 

Finally,  the  effects  of  dynamics  and  hydrodynamics  on  gait  operation  are  not 
covered.  This  model  encompasses  only  the  kinematic  features  of  AquaRobot.  This  means 
the  model  imposes  no  limits  on  vehicle  acceleration.  Similarly,  although  the  manipulator 
boom,  tether,  lifting  lines,  and  transponder  systems  were  cursorily  described,  they  are  not 
modeled  nor  considered  in  this  thesis. 

C.  PROBLEM  STATEMENT 

This  thesis  concentrates  on  designing  an  improved  alternating  tripod  gait  for 
AquaRobot.  Simulation  of  the  improved  design  is  an  inherent  design  goal.  Other  specific 
design  goals  include; 

♦  smooth  leg  motion, 

♦  omnidirectional  body  movement, 

♦  continuous  body  motion,  and 

♦  limited  path  following  capability. 

The  laboratory  director  from  Japan's  PHRI  agreed  that  implementing  these  improvements 
in  an  alternating  tripod  gait  design  would  provide  immediate  increased  effectiveness  over 
the  existing  design  [TAK93]. 
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The  selection  of  a  gait  is  a  very  difficult  problem.  McGhee  [MCG68a]  showed  that 
39,916,800  possible  nonsingular  periodic  hexapod  gaits  exist.  Further,  McGhee  [MCG85] 
suggests  the  total  number  of  hexapod  gaits  is  actually  a  larger,  unknown  number.  This 
thesis,  however,  is  restricted  to  a  single  type  of  gait  sequence  known  as  the  tripod  gait. 
The  tripod  gait  is  a  singular  gait  because  three  legs  (hence,  the  term  tripod)  are  placed 
simultaneously.  The  gait  alternates  between  sets  of  three  legs.  LEGs  (1,  3,  5)  are 
considered  tripod  zero  (TRIPODO)  and  LEGs  (2,  4,  6)  are  considered  tripod  one 
(TRIPOD  1).  This  zero  and  one  terminology  is  in  keeping  with  C  and  C++  programming 
conventions. 

The  alternating  tripod  gait  described  above  is  particularly  important  for  walking 
machines,  and  is  another  reason  for  the  prevalence  of  hexapod  vehicles  [TOD85).  Sbc  legs 
always  allows  a  supporting  tripod,  even  when  half  the  legs  are  in  the  transfer  phase. 
Therefore,  it  allows  reasonable  walking  speed  while  always  maintaining  static  stability. 

D.  SUMMARY 

This  chapter  outlined  the  geometrical  features,  physical  constraints,  and  model 
simplifications  of  the  AquaRohot  underwater  walking  robot.  Next,  the  scope  of  the  work 
this  thesis  undertakes  was  described.  The  following  chapter  contains  the  development  and 
implementation  of  the  kinematic  and  kinematics-related  concerns  of  the  AquaRobot  when 
applied  to  an  alternating  tripod  gait  design. 
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IV.  KINEMATICS 


In  this  chapter,  we  consider  the  kinematics  problem  of  computing  the  position  and 
orientation  of  AquaRoboi's  foot,  relative  to  the  center  of  the  body,  given  the  joint  angles 
for  the  HEP,  KNEEl,  and  KNEE2.  Next,  we  investigate  the  more  difficult  inverse 
kinematics  problem;  ^ven  the  desired  position  and  orientation  of  the  foot  relative  to  the 
body  center,  compute  the  set  of  joint  angles  which  will  achieve  that  result.  Then  a  method 
of  transforming  between  the  world  and  body  coordinate  systems  is  derived.  Next,  the 
concept  of  workspace  and  its  effects  on  walking  is  investigated.  Kinematics,  inverse 
kinematics,  coordinate  transformation,  and  workspace  discussions  are  all  developed  using 
methods  from  Craig  [CRA86].  The  chapter  ends  Avith  a  discussion  of  the  various  body 
postures  used  in  this  thesis. 

A.  KINEMATICS 

A  typical  manipulator,  in  this  case  a  leg  for  AquaRobot,  is  actually  a  set  of  bodies 
connected  in  a  chain  by  joints.  The  bodies  are  called  links.  For  the  purposes  of  kinematics, 
links  are  considered  only  as  rigid  bodies  which  define  the  relationship  between  two 
neighboring  joint  axes.  Joints,  then,  form  the  connection  between  neighboring  links.  Joints 
are  typically  classed  as  either  revolute  or  prismatic.  Revolution  about  some  axis  describes 
a  revolute  joint.  Sliding  action  along  some  axis  describes  a  prismatic  joint.  All  of 
AquaRobot' s\o\TAs  are  revolute.  [CRA86] 

At  the  end  of  the  chain  of  links  that  make  up  the  manipulator  is  the  end-effector.  In 
our  case,  the  end-effector  is  the  foot  at  the  end  of  the  leg.  Specifically,  an  end-effector  for 
AquaRobot  is  a  point  foot  at  the  end  of  LINK3  on  each  leg.  The  position  of  the  foot  with 
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respect  to  the  center  of  the  body  is  described  by  defining  a  foot  {FOOT}  frame  relative  to 
the  center-of-body  {CB}  frame.  A  frame  is  the  set  of  four  vectors  giving  position  and 
orientation  information.  Three  of  the  vectors  are  typically  unit  vectors  defining  the 
principal  axes  of  the  fi'ame.  The  other  vector  locates  the  origin  of  the  fiame  with  respect 
to  some  other  frame,  including  the  origin  of  the  world  {W}  fi'ame.  [CRA86] 

Four  quantities  are  required  to  kinematically  describe  a  manipulator.  Of  these,  two 
describe  the  link  and  two  describe  the  link's  connection  between  adjoining  links.  For 
revolute  joints,  is  known  as  the  joint  variable  and  the  other  three  quantities  are  known 
as  link  parameters.  Mechanisms  defined  with  these  conventions  are  typically  described 
using  Denavit-Hartenberg  (DH)  notation  {DEN55].  There  are  many  related  conventions 
that  ascribe  to  the  name  Denavit-Hartenberg.  In  this  thesis,  fi'ame  {/}  is  attached  to  link  i 

and  has  its  origin  lying  on  joint  axis  i.  In  other  words,  link  /  follows  inboard  joint  i. 
[CRA86] 

Figure  4-1  is  a  kinematic  detail  of  a  typical  leg  firom  the  center-of-body  to  the  foot. 
Leg  fi-ames  are  defined  according  to  their  respective  joints.  The  (CB)  fi'ame  corresponds 
to  an  imaginary  fi'ame  zero  {0}.  Frame  {0}  does  not  move  and  is  considered  the  reference 
fi'ame  for  AquaRobot.  Frame  { 1 }  is  not  actually  a  fi'ame;  however,  for  ease  of  performing 
the  kinematics,  each  leg  is  treated  as  if  it  were  a  rotation  of  60°  about  JOESTTO.  Therefore, 
LEGl  is  at  0°,  LEG2  is  at  60°,  LEGS  is  at  120°,  etc.  With  this  in  mind.  Figure  4-1  shows 
the  progression  of  frames  from  inboard  outward  as  {CB},  {0},  {HIP},  {KNEEl}, 
{KNEE2},  and  {FOOT}.  Recall  that  JOINT4  and  the  entire  foot  is  not  used. 

The  leg  itself  is  referenced  to  two  other  fi'ames.  First  is  the  center-of-gravity  {CG} 
frame.  This  frame  locates  the  center  of  gravity  of  AquaRobot,  which  is  not  necessarily  the 
same  point  as  the  center  of  body.  For  our  purposes,  {CG}  is  set  equal  to  {CB}.  Then 
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{CG}  is  referenced  to  {W}.  The  {CG}  and  {W}  frames  are  shown  only  for  perspective 
and  are  not  used  in  the  derivation  of  leg  kinematics.  Transformations  between  {W}  and 
{CB},  however,  are  examined  later  in  this  chapter. 

The  general  form  of  the  transformation  that  relates  the  frames  attached  to 
neighboring  links  using  the  joint  variable  and  link  parameters  previously  defined  is 

cos^J  -sin^  0  a._, 

sin  6^  cos  cos  cos  a^_^  -  sin  a,_,  -  sin 

sin  sin  a,_,  cos^  sin  cosa,_,  cosa^.i^/, 

0  0  0  1 


where 

is  the  angle  between  Z,_,  and  Z,  measured  positive  about  Z,_,, 
is  the  distance  from  Z^_^  to  Z,  measured  along  A',,, 
d^  is  the  distance  from  to  measured  along  Z^,  and 
^  is  the  angle  between  X^_^  and  X^  measured  about  Z^ . 

Table  4-1  lists  the  link  parameters  for  a  representative  leg.  Once  the  link  frames  are 
defined  and  the  corresponding  link  parameters  found,  values  from  Table  4-1  are  used  in 
equation  4.1  to  compute  the  transformations  for  each  link.  The  {CB}  to  {0} 
transformation  results  in 


'Co  -5-0  0  O' 

CBj>_  ^0  ^0  0  0 

0  0  10’ 
0  0  0  1_ 


(4.2) 


where  Q  represents  cos^o  represents  sin  Oq.  Note  that  the  distance  a_,  between 

(CB)  and  (0}  is  zero  centimeters  because  these  two  frames  overlap.  The  {0}  to  {1} 
transformation  results  in 
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(4.3) 


0 

«o' 

7- 

C. 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

where  is  the  distance  between  {0}  and  {!),  37.5  centimeters. 


Table  4-1.  Link  Parameters  yokAquaRobot. 


i 

(X  , 

(degrees) 

(cm) 

d, 

(cm) 

0. 

(cm) 

Range 

-1 

^trORLD 

^WORLD 

dwORLD 

^WORLD 

0 

0 

0 

0 

^0  =0“.60°.120“,180®,240“,300° 

1 

0 

0 

B, 

2 

-90 

0 

B, 

3 

0 

^2 

0 

B, 

02=50.0,  -156.4°<^3  S23.6° 

4 

0 

^3 

0 

B, 

The  other  transformations  follow  the  same  pattern; 


-$2  0  a, 

0  1  0 
-Q  0  0 
0  0  1 


(4.4) 


-5j  0  flj 

Cj  0  0 

0  1  0 

0  0  1 


(4.5) 
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and 


Q  ~S^  0  a, 

3  5,  Q  0  0 

*  ~  0  0  10 

0  0  0  1 


(4.6) 


Because  the  entire  foot  is  not  used,  we  are  not  concerned  with  the  action  of  JOINT4. 
Therefore,  setting  ^4  =  0  produces  a  substitution  for  equation  4.6; 


1  0  0 


0  0  0 


(4.7) 


which  provides  the  necessary  transformation  from  KNEE2  to  the  FOOT.  The  final  matrix 
used  to  determine  the  transformation  from  (CB)  to  {4}  is  found  by  multiplying  each 
of  the  individual  transformation  matrices  together: 

(4.8) 


The  derivation  of  the  final  transformation  matrix,  including  intermediate  results,  is 
shown  in  equations  4.9  through  4. 12. 


(4.9) 


Coi 

Cq, 

0 

^qCq 

’°r= 

•Jo, 

*Soi 

0 

^0*^0 

1  u 

1 

0 

0 

0 

0 

> 

0 

0 

0 

1 

CoiQ 

■^01*^2 

^oCq  "^^jCoj 

<5oiQ 

Co, 

^0*^0  '*"^,^01 

2'*  O'*  r  2* 

-Q 

0 

- 

0 

0 

0 

1 

(4.10) 
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(4.11) 


^01*^23 

^0^0  ^01  (^1  ^^2 ) 

5o,C23 

-‘^01*^23 

Q. 

^0*^0  *^01  (^1  ^2^2  ) 

0 

-a, 5, 

0 

0 

0 

1 

and 


(^01*^23 

-*50. 

®3^01^23  "^^0^0  "^^01(^1 

*^01^23 

_C  <’ 
*J0I‘-’23 

^3*^01^23  ^0*^0  *^01  (^1 

+  ^2Q) 

-‘^23 

0 

"^3*^23  -  ^2*^2 

y 

0 

0 

0 

1 

where 

q  =COS0^, 

5,  =  sin 

q,=qc, -5,5- co4^  +  0j, 

and  5„  =  5,q  -  =  sin(  ^  +  6^). 


Two  Matlab  programs  were  written  to  verify  the  derivation  of  the  kinematics 
transformations.  Program  jcoord.m  uses  equation  4. 12  to  compute  the  (x,  y,  z)  coordinates 
of  the  HEP,  KNEEl,  KNEE2,  and  FOOT  from  joint  angles  the  user  inputs.  The  Matlab 
program  kin.m  is  a  function  that  provides  a  similar  output  to  the  c3.m  program  when 
called.  These  two  code  modules  are  included  in  Appendix  B. 


B.  INVERSE  KINEMATICS 

In  the  first  section  of  this  chapter,  we  focused  on  the  direct  Jdnematics  of  the  leg. 
That  is,  we  computed  the  position  and  orientation  of  {4}  with  respect  to  {CB}.  In  this 
section,  we  focus  on  the  inverse  kinematics  of  the  leg.  Our  goal  here  is  to  find  the 
required  set  of  joint  angles  to  place  the  foot,  relative  to  the  center  of  the 

body,  when  the  desired  foot  position  and  orientation  are  known.  There  are  two 


37 


approaches  to  solving  the  inverse  kinematics  problem.  The  algebraic  approach  is  basically 
one  of  manipulating  the  link  parameters  into  kinematic  equations  of  a  form  for  which  a 
solution  is  known.  The  geometric  approach  is  to  try  to  decompose  the  spatial  geometry  of 
the  leg  into  several  lesser  plane  geometry  problems.  The  geometric  approach  is  the 
method  used  here.  Figure  4-2  shows  the  plane  geometry  associated  with  AquaRohot. 
[CRA86] 

First,  consider  the  triangle  formed  by  the  CB,  HIP,  and  FOOT  of  representative 
LEG2  and  shown  in  the  plan  view  of  Figure  4-2.  We  can  apply  the  Law  of  Cosines  to 
solve  for  0^ : 

pI  +pI  =aQ  +bf  -lajb^  cos(l80°-^j)  =  a^  +b^  +2aoi,  cos^,.  (4.13) 


The  foot  position  is  given  in  body-fixed  coordinates  (p^  ,p  ,Pj.  ).  Then,  we  have 


cos^i  =C,  = 


^2  r  i2 

Px  Py  ^0  ^1 

2«o*, 


(4.14) 


where 


*1  =  }l(Px  -"o  cos^o)'  +(py -Oo  sin  Oof 


t-15) 


Finally,  we  use  the  two-argument  arctangent  (Atan2)  to  find 

=Atan2(±Vl-C^C,). 


(4.16) 


Now  we  consider  the  triangle  formed  by  the  KNEEl,  KNEE2,  and  FOOT  joints  as 
shown  in  the  cross-sectional  view  of  Figure  4-2.  First,  we  find  expressions  for  angles  P 
and  \j/,  where 

(4.17) 
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Figure  4-2.  Plane  Geometry  of  AquaRobot. 
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Then, 


tan^=|*-, 

and  =  Atan2(/)^ ,  \ ), 

where 

*2=*, -a,. 

Applying  the  Law  of  Cosines  again  to  find  Y- 

al  =al  +b^  - la^b  cos 


Then,  we  have 


cosy^= 


2^2*3 


and  sin  y/  =  ±-y/ 1  -  cos^  y/, 

where 

b,=ylbl+pl 

Now,  substituting  equations  4.18, 4.22,  and  4.23  into  equation  4. 17  yields 

0^  = 


and 


02  -  Atan2(±-y/l-cos^  if/,  cos  t/]  -  Atan2(/?, ,  ). 


(4.18) 

(4.19) 

(4.20) 

(4.21) 

(4.22) 

(4.23) 

(4.24) 

(4.25) 

(4.26) 
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Finally,  we  again  apply  the  Law  of  Cosines  to  the  triangle  formed  by  the  KNEEl, 
KNEE2,  and  FOOT  joints  to  solve  for  0^ ; 

b]  =  +  a;  -  cos(  1 80®  -  ^3 )  =  +  2a^a^  cos  0^.  (4  27) 


We  then  have 


cosdj  -  Cj  = 


2aM-> 


(4.28) 


and 


^3  =  Atm2(±yll-Cl,C,). 


(4.29) 


Because  equations  4.16,  4.26,  and  4.29  include  the  possibility  of  a  positive  or 
negative  result  (±),  there  are  eight  potential  solutions  for  a  single  goal.  Physical  joint 
limitations  may  prevent  using  some  of  the  potential  solutions.  Of  the  remaining  valid 
solutions,  the  one  closest  to  the  present  leg  configuration  is  generally  chosen.  Still,  all 
chosen  solutions  are  tested  using  the  direct  kinematics  routines  previously  developed.  If 
the  resulting  kinematics  solution  yields  a  foot  position  equal  to  the  desired  foot  position, 
we  accept  the  inverse  kinematics  solution  as  the  real  solution.  We  have  solved  for  0^,  0^, 
and  0j .  The  solution  for  0^  is  already  known  because  it  is  the  angle  that  determines  which 
leg  is  selected.  For  example,  if  we  know  we  are  trying  to  place  FOOT2,  then  we  know  0^ 
is  at  60°.  This  is  because  the  HIP  joints  are  located  at  constant,  known  angles  about  the 
body. 

C  COORDINATE  TRANSFORMATIONS 

The  AquaRobot  simulation  uses  two  coordinate  systems  in  its  calculations,  the  world 
coordinate  system  and  the  body  (xg^yg,^^)  coordinate  system.  The  world 

coordinate  system  is  used  when  it  is  necessary  to  specify  absolute  positions  or  velocities  of 
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the  body  center,  feet,  or  terrain.  The  world  coordinate  system  is  defined  with  the  z^-axis 
positive  downward,  with  the  %'axis  positive  when  pointing  North,  and  the  y^-axis 
positive  when  pointing  East. 


The  body  coordinate  system,  composed  of  three  dimensional  local  coordinates  fixed 
to  the  robot's  body,  is  used  to  describe  the  coordination  between  the  body  and  the  legs  and 
to  determine  stability.  The  origin  of  the  body  coordinate  system  is  defined  as  the  center  of 
the  plane  defined  by  the  HIPs  of  all  sbc  legs.  The  positive  x^-axis  lies  along  the  line  joining 
the  CB  and  the  HEP  of  LEGl.  The  positive  perpendicular  to  the  x^-axis  at  the 

center  of  the  body  and  passes  between  LEG2  and  LEG3.  The  z^-axis  lies  orthogonal  to 
the  Xg~  and  >>5 -axes  with  positive  z^  pointing  downward. 

The  method  of  transforming  between  the  body  coordinate  system  and  the  world 
coordinate  system  is  defined  through  the  use  of  a  4  x  4  homogeneous  transformation 
nidcrix,  [CRA86].  The  matrix  may  be  partitioned  into  sub  matrices 


Wn  Wp 


(4.30) 


where  is  a  3  x  3  rotation  matrix  and  '^Pg  is  a  3  x  1  position  vector  [LEE88a].  The 
vector  '^Pg  represents  the  position  of  the  body  in  the  world  coordinate  system  and 
gives  the  rotation  of  the  body  relative  to  the  world  coordinate  system.  The  p?  rotation 
matrix  is  derived  using  the  X-Y-Z  fixed  angle  orientation  convention  [CRA86].  Fixed 
angles  mean  the  rotations  are  specified  about  a  non-moving  reference  system.  For  our 
purposes,  rotations  about  the  x-axis  are  known  as  roll  ((}>),  rotations  about  the  y-axis  are 
known  as  pitch  or  elevation  (0),  and  rotations  about  the  z-axis  are  known  as  yaw  or 
azimuth  (ij/).  The  rotation  matrix  then  becomes 
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Wn 

B'^xrzUe.r) 


S^gSg+C^C^  — 

—Sg 


(4.31) 


Xyf 

Vw 

yB 

^B 

1 

1 

The  ^  matrix  allows  transformation  from  body  coordinates  to  world  coordinates 
using  the  relationship 


(4.32) 


where  the  position  vectors  [x^  l]’^  and  [x^  Zg  l]^  describe  the  same 

point  in  space  in  world  and  body  coordinates,  respectively.  The  Matlab  code  module 
b2wtrans.m,  found  in  Appendix  B,  performs  the  body-to-world  transformation. 

To  convert  from  world  coordinates  to  body  coordinates  requires  a  similar 
transformation.  In  this  case,  the  inverse  of  the  p"  matrix  is  used.  Then,  using  the  form  of 
equation  4.32,  we  find  the  new  transformation  process  is 


(4.33) 


ys 

-^7^1 

yw 

^B 

z^ 

1 

1 

The  Matlab  code  module  w2btrans.m,  found  in  Appendix  B,  performs  the  world-to-body 
coordinate  transformation. 

D.  WORKSPACE 

In  this  section,  AquaRobot's  workspace  is  computed.  Workspace  is  that  volume  of 
space  which  an  end-effector,  in  our  case,  a  foot,  can  reach  [CRA86].  The  workspace  is 
also  known  as  the  working  volume  The  mechanical  limits  of  the  joints  restrict  leg  motion 
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and  are  a  major  factor  to  consider  when  developing  control  algorithms  for  a  legged  vehicle 
[LEE88a].  Because  each  leg  of  AquaRohot  has  the  same  geometrical  configuration  and 
joint  limits,  the  working  volumes  of  the  legs  are  identical.  The  limits  of  the  joint  variables 
for  a  representative  LEG  are  shown  in  Table  4-2. 


Table  4-2.  Representative  LEG  Joint  Limits. 


LEG  Angles  (LEGl  -  LEG6) 

=0°, 60°, 120°, 180°, 240°, 300° 

HEP  Joint  Limits 

-60°  <  0,  <  60° 

KNEEl  Joint  Limits 

-lO6.6°<0j  <73.4° 

KNEE2  Joint  Limits 

-156.4°<  ^3  <23.6° 

FOOT  Joint  Limits 

-45°<6>4  ^45° 

These  joint  variable  limits,  then,  separate  the  reachable  area  from  the  unreachable  area 
[MCG79].  Reachable  areas  move  with  the  body.  The  region  included  within  the  reachable 
area  is  known  as  the  unconstrained  working  volume  (UWV). 

The  constrained  working  volume  (CWV)  is  defined  as  a  subset  of  the  original 
working  volume,  for  each  leg,  that  ensures  static  stability.  Therefore,  the  CWV  sets  soft 
limits  for  each  leg  so  as  to  exclude  points  from  the  working  volume  that  may  lead  to 
instability  [LEE88b].  In  our  case,  the  working  volume  is  also  constrained  to  prevent  leg 
collisions.  An  excluded  area  for  AquaRobofs  legs,  then,  is  that  part  of  the  reachable  area 
where,  if  a  foot  were  placed  there,  instability  or  leg  collision  might  result  [MCG79]. 

1.  Unconstrained  Workspace 

The  unconstrained  horizontal  workspace  of  AquaRobot  is  shown  in  Figure  4-3. 
The  reachable  areas  include  the  sections  in  the  plane  around  the  individual  HIPs  and 
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within  the  mechanical  joint  limits.  The  unconstrained  vertical  workspace,  or  Z-plane 
reachable  area,  depends  on  the  height  of  the  vehicle's  center-of-body  above  the  terrain  and 
is  not  shown  in  Figure  4-3. 

To  define  the  unconstrained  vertical  workspace,  we  must  define  the  maximum 
and  minimum  reach  of  AquaRobot.  If  a  leg  were  extended  to  its  fuUest,  the  added  lengths 
of  LINKO  (37.5  cm),  LINKl  (20.0  cm),  LINK2  (50.0  cm),  and  LINK3  (100.0  cm)  would 
equal  207.5  centimeters.  This  length  does  not  include  the  diameter  (25.0  cm)  or  the 
thickness  (3.5  cm)  of  the  footpad.  However,  because  of  the  45°  angle  restriction  on  the 
foot  joint,  the  maximum  reach  of  AquaRobot  is  as  shown  in  Figure  4-4.  The  CB,  when  the 
foot  is  at  its  maximum  unconstrained  reach  of  178.2107  centimeters,  is  a.  /0.2107 
centimeters  off  the  terrain.  Figure  4-4  includes  an  illustration  of  what  would  be  the 
unconstrained  vertical  workspace  if  the  maximum  reach  were  used. 

If  the  45°  foot  joint  angle  is  used  again  as  the  restricting  dimension,  it  is  possible 
for  the  minimum  unconstrained  reach  of  AquaRobot  to  become  negative,  in  the  sense  that 
the  foot  can  pass  completely  under  the  CB,  as  shown  in  Figure  4-5.  This  range  of  reach 
allows  greater  flexibility  when  designing  a  gait,  but  significantly  increases  the  complexity 
of  the  gait  algorithms  because  leg  collision  avoidance  must  be  considered.  Figure  4-5 
includes  a  drawing  of  what  would  be  the  unconstrained  vertical  workspace  if  the  minimum 
reach  were  used. 

Three  Matlab  programs  were  written  to  verify  the  derivation  of  the  kinematics 
transformations.  Program  ucwlinkl.m,  the  results  of  which  are  shown  in  Figure  4-6, 
calculates  and  plots  the  unconstrained  workspace  of  LINKl,  the  HIP  to  KNEEl  link. 
Figure  4-7  shows  the  results  of  program  ucwlink2.m,  which  calculates  and  plots  the 
unconstrained  workspace  of  LINK2,  the  KNEEl  to  KNEE2  link.  Program  ucwlink3.m, 
the  results  of  which  are  shown  in  Figure  4-8,  calculates  and  plots  the  unconstrained 
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Figure  4-3.  Unconstrained  Horizontal  Workspace. 
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Figure  4-4.  Maximum  Reach  and  Associated  Unconstrained  Vertical  Workspace. 
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Figure  4-5.  Minimum  Reach  and  Associated  Unconstrained  Vertical  Workspace. 
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Figure  4-6.  LINKl  Unconstrained  Workspace. 
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Figure  4-7.  LINK2  Unconstrained  Workspace. 
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workspace  of  LINK3,  the  KNEE2  to  FOOT  link.  These  three  code  modules  are  included 
in  Appendix  B. 

2.  Constrained  Workspace 

Now  that  the  UWV  is  defined,  we  can  define  the  CWV.  The  CWV  used  in  this 
thesis  results  fi'om  six  basic  constraints; 

♦  the  CB  height  is  fixed  at  the  point  where  the  maximum  reach  is  achievable, 

♦  the  maximum  and  minimum  reach  meet  the  45°  foot  joint  angle  requirement, 

«  the  terrain  is  flat, 

♦  the  minimum  reach  is  set  equal  to  the  radius  the  HIPs  are  fi^om  the  CB, 

«  the  legs  are  not  allowed  to  collide  or  overlap,  and 

»  the  area  encompassed  by  the  footpads  is  included  in  case  feet  are  added  in  future 
work. 

Start  with  the  maximum  reach  obtained  when  the  CB  is  at  70.2107  centimeters.  If  the  CB 
is  then  constrained  to  this  value,  the  minimum  reach  becomes  36.79  centimeters.  For 
simplification,  we  make  the  minimum  reach  equivalent  to  the  radius  the  HBPs  are  fi'om  the 
CB;  37.5  centimeters.  The  resulting  maximum  and  minimum  constrained  reach  is  shown  in 
Figure  4-9  and  represents  the  construed  vertical  workspace.  The  height  that  the  leg  can 
be  raised  is  obviously  restricted  when  the  CB  height  is  fixed.  However,  this  factor  is  not  as 
important,  when  using  flat  terrain,  as  reducing  the  number  of  variables  when  first 
implementing  a  control  program.  Changing  the  CB  height  to  allow  traversal  of 
unstructured  terrain  is  easily  accomplished  if  required  in  future  research. 

Figure  4-10  shows  the  constrained  horizontal  workspace.  The  constrained 
horizontal  workspace  is  developed  using  LEGl,  LEG2,  and  LEG6  as  an  example.  Each 
LEG  is  drawn  with  its  centerline  beginning  at  the  CB  and  bisecting  the  ±  60°  joint  limit  of 
its  HIP.  The  boundary  for  the  excluded  areas  is  selected  as  lines  that  separate  the 
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reachable  areas  for  the  three  legs  exactly.  The  actual  exclusion  areas  are  then  chosen  based 
on  the  edge  of  the  footpad  touching  these  separation  lines.  This  method  allows  the 
addition  of  the  footpads  in  the  future  without  changing  the  workspace  algorithms.  The 
workspace  boundaries  then  form  the  sides  of  a  section  of  an  annulus  circumscribed  about 
the  CB.  The  inner  boundary  of  the  annulus  is  the  radius  the  HIPs  make  about  the  CB.  The 
outer  boundary  is  the  radius  of  the  maximum  reach  of  a  LEG  about  the  CB. 

3.  Strategy  for  Constrained  Workspace  Use 

The  primary  purpose  of  the  constrained  workspace  is  to  define  the  limits  for 
determining  the  possible  length  of  the  stride.  If  an  arbitrary  direction  is  chosen  for  a 
representative  foot  to  travel,  that  foot  will  intersect  the  edge  of  the  constrained  workspace 
at  some  point.  Determining  the  point  of  intersection  requires  algorithms  to  find  the 
intersection  of  a  directed  line  and  a  line  segment  and  the  intersection  of  a  directed  line  and 
a  section  of  a  circle.  Further,  if  an  intersection  is  found,  it  must  be  the  intersection  at  the 
correct  orientation  of  the  directed  line.  Development  and  implementation  of  methods  to 
generate  the  required  results  are  abstracted  from  Kanayama  [KAN92]. 

First,  we  develop  the  method  to  determine  the  intersection  of  two  directed  lines. 
In  our  case,  one  directed  line  is  derived  from  the  current  foot  position  and  desired 
direction  of  travel.  The  second  directed  line  is  defined  as  a  side  of  the  constrained 
workspace  for  a  given  leg,  LEGl.  Referring  to  Figure  4-11,  we  choose  an  arbitrary 
direction  of  0,  for  the  foot  to  travel  within  the  LEGl  workspace.  The  current  position  of 
the  foot  is  presumed  to  have  coordinates  Pp  ={xg,yg),  with  no  Zg  component  because 
only  the  horizontal  workspace  is  of  interest  at  this  point.  The  directed  line  is  then 
formed  using  the  expression 

(4  34) 
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where  x,  =  Xg  and  >',  =  >^5 . 

We  know  tne  endpoints  of  the  line  segments  that  form  the  sides  of  the 
constrained  workspace  for  LEGl.  Using  the  known  endpoints  and  the  two-argument 
arctangent  (Atan2)  function,  it  is  an  easy  task  to  find  the  angle  the  line  segments  make 

with  the  body-fixed  coordinate  system.  If  the  angle  for  one  side  is  taken  as  0.  ,  the  second 
directed  line  becomes 

(4.35) 

If  we  let  an  intersection  be  ix,y),  the  distance  between  the  intersection  point  and  either  of 
the  two  directed  lines  is  zero. 

(^->',)cos^, -(x-x,)sin  =  0  (4.36) 

(>'“>'2)cos02 -(x-X2)sin  ^2  =  0  (4.37) 

Combining  equations  4.36  and  4.37  i'*  0  matrix  form; 

sin  -  cos  T X  Xj  sin  -  >^1  cos  0^ 

sin  02  -  cos  02  X2  sin  02  -  >'2  cos  02 

Then,  solving  equations  4.36  and  4.37  simultaneously  yields  the  intersection  point 

( X,  sin  -  jv,  cos  )  -  cos  0, 

( Xj  sin  ^2  ~  >^2  cos  ^2 )  -  cos  02 

(4.39) 

_  -  cos  02(^1  sin  0,  -_y,  cos0)-(-cos0,(x2  sin  02  -y2  cos 02) 

sin(02  -  0,) 

and 


1 

'"^'~sin(02-0,) 


(4.38) 
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Figure  4-11.  Intersection  of  Directed  Line  and  Line  Segment. 
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sin£’, 
sin  02 


(x,  sm^i  -y^  cos^i) 
(xjSin^j  -y^  cos 02) 


sin  0^{x2  sin  ->'2  cos^j)'  sin  ^2(^1  sin  cos^J 

sin(^2  ~  ^1) 


(4.40) 


The  intersection  point  is  then  tested  to  ensure  it  lies  within  the  desired  segment 
of  the  directed  line  forming  the  boundary  of  the  constrained  workspace.  Kanayama 
proposes  that  for  any  three  arbitrary  distinct  points  Pi-[^i,yi),  nnd 

Pj  =  (xjj^j)  on  a  directed  line,  /?,  is  upstream  of  P2  and  P2  is  upstream  of  p^  if  and  only 
if 

X,  cosa+^i  sin  a  <  Xj  cosa+y2  sin  a  <  X3  cosa+yj  sin  a,  (4.41) 

where  a  is  the  orientation  of  the  directed  line  that  includes  the  three  points  [KAN92].  The 
endpoints  for  the  line  segment  in  question  are  substituted  for  points  />,  and  p^  and  the 
intersection  point  is  substituted  for  point  Pj  equation  4.41.  Alpha  (a)  then  becomes  the 

angle  associated  with  the  directed  line  that  makes  up  the  side  of  the  constrained 
workspace.  From  these  values,  we  determine  whether  the  intersection  point  is  within  the 
endpoints  of  the  line  segment  of  interest.  The  Matlab  program  segint.m,  found  in 
Appendix  B,  performs  the  segment  intersection  calculations  described  above. 

Now  we  turn  our  attention  to  developing  the  method  to  determine  the 
intersection  of  a  directed  line  and  a  circular  section.  Again,  a  directed  line  is  derived  from 
the  current  foot  position  and  desired  direction  of  travel.  The  circular  section  is  defined  as 
either  the  inner  or  outer  radius  of  the  constrained  workspace  for  a  given  leg,  LEGl.  In  this 
case,  we  must  test  for  an  intersection  and  then  determine  whether  the  intersection  is  within 
the  section  of  interest.  Referring  to  Figure  4-12,  we  choose  an  arbitrary  direction  of  a  for 
the  foot  to  travel  within  the  workspace.  The  coordinates  and  orientation  of  the  foot  are 
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then  known;  Pp  ={(a,i>),a}.  The  radius  and  center  of  the  circle  of  interest  are  then 
defined,  respectively,  as  r  and  Pc  ~(^c>>'c)  Front  these  values,  we  can  determine  the 
perpendicular  distance  d  fi-om  to  the  directed  line  previously  described  byp,..  using  the 
relationship 

d  =  {yc-b)cosa-{x(;-a)sma.  (4.42) 


The  radius  r  and  the  perpendicular  distance  d  then  make  up  two  sides  of  a  right 
triangle  with  side  /  unknown.  Side  /  is  found  using  the  Pythagorean  Theorem; 


(4.43) 


Next,  the  point  p,  =  where  d  and  /  intersect  is  found  using 


X,  =  Xf,  ^■dco. 


{f-)= 


x^  +ifsina 


(4.44) 


3^1  =  >^0  “ sini  ^  -  a  I  =  >'c  - cos  a. 


(4.45) 


The  results  of  equations  4.43,  4.44,  and  4.45  are  then  used  to  find  the  intersection  point  of 
the  directed  line  and  the  circular  section  using 

Xi„„rcp,^x,+lcosa  (4.46) 

>'.m.«,pr  =  >'i+/sina  (4.47) 

if  the  foot  is  located  inside  the  diameter  of  the  circle  and 

^in«.«.pr  =  ^i-^cosa  (4.48) 

Jmr«rcp,=>'i-^cosa  (4.49) 

if  the  foot  is  located  outside  the  diameter  of  the  circle. 
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Because  the  ends  of  the  circular  sections  of  the  constrained  workspace  are 
known,  the  intersection  point  is  then  tested  to  ensure  it  lies  within  the  section  of  interest 
using  the  method  described  in  equation  4.41.  The  Matlab  program  arcint2.m,  found  in 
Appendix  B,  performs  the  arc  intersection  calculations  just  described. 

Once  the  intersections  of  the  various  feet  and  their  respective  workspaces  are 
known,  the  maximum  stride  is  determined.  The  maximum  stride  is  defined  as  the  minimum 
of  the  maximum  strides  of  the  individual  feet  in  a  given  tripod.  Using  the  maximum  stride 
allows  all  three  feet  in  the  tripod  to  move  the  same  distance  in  the  same  amount  of  time. 
Figure  4-13  illustrates  finding  the  maximum  stride  possible,  given  the  desired  direction  of 
travel  is  -45°  and  the  tripod  selected  is  TRJPODO  (LEGs  1,  3,  5).  The  Matlab  program 
maxd25.m,  found  in  Appendix  B,  determins  the  maximum  stride. 

E.  TERRAIN  AND  POSTURES 
1.  Terrain  Considerations 

A  walking  machine  is  expected  to  operate  in  structured  or  unstructured  terrain. 
The  terms  structured  and  unstructured  are  too  diverse  to  use  when  describing  the  possible 
classification  and  description  of  terrain.  Hirose  suggests  using  five  different  types  of 
terrain  classification,  as  shown  in  Figure  4-14.  First,  the  O  type  terrain  is  a  surface  which 
supports  standard  rhythmical  walking.  The  flat  terrain  model  used  in  this  study  is  of  type 
O.  Second,  the  H  type  terrain  includes  deep  holes  in  which  the  legs  of  the  walking  machine 
can  not  reach  bottom.  A  walking  machine  can  not  choose  the  holes  as  possible  footholds, 
but  can  step  over  the  holes.  Next,  the  P  type  terrain  is  described  as  having  poles  or  rocks 
higher  than  the  walking  vehicle  itself  For  P  type  terrain,  the  walking  machine  must  avoid 
trying  to  cross  over  the  poles  and  rocks.  HP  terrain  is  a  combination  of  the  H  and  P  terrain 
types.  Finally,  the  G  type  is  considered  the  usual  rough  terrain.  [HIR86a] 
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Figure  4-13.  Determination  of  Maximum  Stride. 


Figure  4-14.  Terrain  Classification  (After  [HIR86a]). 
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Basically,  the  terrain  models  for  AquaRobot  fall  within  three  general  categories: 
1)  essentially  flat,  O  type  terrain  for  this  study,  2)  G  type  terrain  with  random  irregularities 
of  ±  35  centimeters  to  simulate  walking  on  an  unfinished  rubble  mound,  and  3)  O  type 
terrain  with  random  undulations  of  ±  5  centimeters  to  simulate  completed  rubble  mound 
walking. 

2.  Operational  Postures 

AquaRobot  was  designed  to  walk  using  body  postures  that  optimize  the 
particular  task  or  function  it  is  performing.  These  modes  of  operation  vary  according  to 
stability,  speed,  and  terrain  requirements.  There  are  essentially  fi\’e  modes  of  operation 
generally  used: 

♦  storage  mode, 

«  initialization  mode, 

«  walking  mode, 

♦  height  and  width  change  mode,  and 

♦  slim  mode. 

AquaRobot  requires  support  when  no  electricity  is  supplied  to  its  joints  because 
there  are  no  brakes.  Hence,  AquaRobot  rides  on  a  mechanical  support  platform  with  its 
legs  folded  so  the  minimum  volume  is  used.  When  AquaRobot  has  its  legs  folded  on  the 
support  platform,  it  is  in  the  storage  mode.  The  storage  mode  posture  is  termed  RESET  in 
this  study.  When  AquaRobot  is  in  the  storage  mode,  the  incremental  encoders  of  the  DC 
motors  are  set  to  an  inhial  position.  The  FOOT  positions  for  the  RESET  posture,  plotted 
from  the  Matlab  program  reset. m,  are  as  shown  in  Figure  4-15.  [IWA87] 

During  the  initialization  mode  of  operation,  AquaRobot  spreads  its  legs,  steps 
off  the  support  platform,  and  assumes  an  arbitrary  START  posture  [IWA87].  We  define 
the  START  posture  in  this  study  based  on  the  maximum  reach  and  the  constrained 
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workspace.  Therefore,  the  START  posture  allows  AquaRobot  to  achieve  the  maximum 
possible  stride  while  maintaining  an  adequate  clearance  over  the  flat  terrain.  The  FOOT 
positions  for  the  START  posture,  plotted  from  the  Matlab  program  start.m,  are  as  shown 
in  Figure  4-16. 

The  walking  mode  is  subdivided  into  two  modes.  1)  flat  terrain  walking  mode, 
and  2)  irregular  terrain  walking  mode.  Only  the  flat  terrain  walking  mode  is  used  in  this 
study.  The  height  and  width  change  mode  allows  changing  the  stance  oi  AquaRobot  based 
on  the  type  of  terrain  and  the  water  speed.  The  tall  posture  is  a  narrower  stance  used  for 
rougher  terrain.  This  means  the  control  algorithm  can  raise  the  feet  higher  to  step  over 
higher  obstacles.  The  short  posture  is  a  wider  stance  used  when  the  current  is  fast.  The 
short  posture  lowers  the  vehicle's  center  of  gravity  and  provides  more  stability  in  these 
situations.  The  tall  and  short  postures  are  attained  simply  through  adjusting  the  vehicle's 
center-of-body  height  over  the  terrain.  [IWA87] 

The  slim  mode,  which  is  not  used  in  this  study,  was  developed  to  allow 
AquaRobo/i.  to  pass  between  closely  spaced  objects.  In  the  slim  mode,  the  body  width  is 
decreased  to  one-half  of  its  original  size  by  rotating  some  legs  so  bilateral  symmetry  is 
achieved.  AquaRobot  takes  on  the  posture  of  a  crab  and  uses  a  slim  version  of  the 
alternating  tripod  gait  to  maneuver.  [IWA87] 

F.  SUMMARY 

The  central  topic  of  this  chapter  was  developing  and  implementing  kinematics  and 
inverse  kinematics  for  AquaRobot.  Next,  coordinate  transformations  required  to  move 
from  body-to-world  and  from  world-to-body  coordinate  systems  were  derived.  Then,  an 
acceptable  constrained  workspace  was  designed.  Finally,  AquaRobot's  postures,  as  applied 
to  various  terrain  types,  were  discussed.  In  the  following  chapter,  static  stability  issues  are 
addressed. 
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Y  Foot  Coordinates  (cm) 


X  Foot  Coordinates  (cm) 


Figure  4-16.  AquaRobot  START  Posture. 
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V.  STABILITY 


A.  INTRODUCTION 

One  of  the  main  considerations  in  walking  machines  is  how  to  achieve  stability,  that 
is,  to  keep  from  falling  over  [TOD85].  In  this  study,  the  static  stability  concept  is 
employed  to  provide  a  simple  basis  for  the  control  of  vehicle  locomotion.  The  concept  of 
stability  mar^  is  known  as  the  yardstick  for  measuring  static  stability.  The  value  is  given 
in  the  relation  between  the  center  of  gravity  and  the  polygon  of  supporting  legs,  and  is 
also  governed  by  the  height  of  the  center  of  gravity  and  compliance  of  the  legs  [HIR86a]. 
For  simplicity,  however,  this  study  uses  the  longitudinal  stabihty  margin  5,  as  the  primary 
approximation  of  stability. 

In  general,  it  is  possible  for  the  center  of  gravity  to  go  outside  the  support  pattern  at 
some  instants  during  locomotion,  even  if  the  support  joints  are  constrained  to  be  inside  the 
working  volumes  of  the  legs  [LEE88b].  However,  there  is  some  flexibility  in  changing  the 
position  and  dimensions  of  the  CWV  inside  the  original  working  volume.  This  flexibility  is 
used  to  increase  vehicle  stability  [LEE88a].  In  our  case,  the  footpad  of  AquaRobot  is 
actually  25  centimeters  in  diameter,  yet  we  are  using  a  point  foot  representation.  This 
gives  us  an  additional  12.5  centimeters  of  stability  safety  margin.  As  such,  mild 
accelerations  and  decelerations  can  be  accomodated  without  changing  the  existing 
algorithms. 

From  the  previous  chapter,  we  know  how  to  find  the  maximum  stride  possible  given 
the  constraints  of  the  workspace  and  the  mechanical  joint  limitations.  It  is  possible  that  the 
maximum  stride  would  allow  the  CG  to  move  beyond  the  support  pattern.  Using  the 


68 


maximum  stride  alone,  then,  would  result  in  unstable  walking.  Therefore,  the  stride  and 
stability  calculations  are  compared  for  each  increment  of  motion  and  the  lesser  of  the  two 
is  chosen  as  the  distance  the  foot  will  travel.  Although  this  method  greatly  reduces  the 
amount  of  motion  flexibility,  it  ensures  stability  and  a  reasonably  fast  gait. 

1.  Center-of-Body  Versus  Center-of-Gravity 

The  center  of  AquaRobot's  body  is  located  at  the  intersection  of  the  plane 
formed  by  its  HEPs  and  the  vertical  center  of  the  torso.  The  CB  remains  constant  during 
any  body  or  leg  motion.  AquaRobot's  center-of-gravity,  however,  changes  with  each 
incremental  motion  of  its  legs  or  body.  Therefore,  at  any  instant  in  time,  the  CG  is  not 
necessarily  located  at  the  CB. 

The  PHRI  had  not  completed  CG  calculations  at  the  time  of  this  thesis  writing. 
Because  the  CG  is  only  needed  for  stability  purposes,  the  CG  is  set  equal  to  the  CB  in  tliis 
study.  Once  the  CG  calculations  are  complete,  the  CG  is  easily  substituted  for  the  CB  in 
stability  algorithms. 

2.  Static  Versus  Dynamic  Stability 

Static  and  dynamic  stability  distinguishes  the  two  types  of  walking  machines  that 
have  been  the  subjects  of  recent  research.  Statically  stable  systems  resist  falling  down  by 
trying  never  to  get  in  a  situation  where  falling  down  is  possible.  Dynamically  stable 
systems  balance  to  keep  from  falling  down.  Statically  stable  machines  have  at  least  four 
legs  and  sometimes  as  many  as  eight,  though  more  typically  six.  Dynamically  stable 
machines  have  from  one  to  four  legs.  The  main  difference  is  the  use  of  balance  in  the 
control  of  body  attitude  in  dynamically  stable  machine^.  [DON87] 

Of  static  and  dynamic  walking,  static  locomotion  is  more  basic  and  less 
complicated  than  dynamic  locomotion  in  terms  of  control  algorithms  and  motion  analysis 
[HIR86a].  It  turns  out  that  insects  and  other  many-legged  creatures  walk  with  statically 
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stable  gaits  [DON87].  AquaRobot  is  an  example  of  an  "insect  type"  machine.  Higher 
animals,  like  horses,  cats,  and  people,  all  walk  using  dynamically  stable  gaits  [DON87], 
The  AquaRobot  simulation  presented  in  this  thesis  uses  a  statically  stable  gait  algorithm. 

B.  STABILITY  MODEL 

The  stability  model  used  in  this  study  is  similar  to  that  proposed  by  McGhee 
[MCG86].  The  longitudinal  stability  margin  S,  is  defined  as  the  shortest  distance  from  the 
body's  center  of  gravity  to  the  boundary  of  the  support  pattern,  measured  in  the  direction 
of  travel.  From  the  definition  of  longitudinal  stability  margin,  a  gait  is  deemed  statically 
stable  if  5,  >  0.  Otherwise,  it  is  deemed  statically  unstable.  Vehicle  stability,  stability 
intercepts,  and  stability  margin  are  detennined  using  the  known  foot  positions  of  the 
supporting  tripod  and  the  known  body  center.  All  stability  calculations  use  the  body-fixed 
coordinate  system  and  substitute  the  CB  for  the  CG. 

Prior  to  planning  any  new  leg  motion,  AguaPobot's  stability  is  evaluated.  The 
evaluation  consists  simply  of  determining  whether  the  vertical  projection  of  the  CB  lies 
within  an  arbitrary  support  pattern  formed  from  the  vertical  projection  of  the  three  feet  in 
a  selected  tripod.  Referring  to  Figure  5-1,  we  see  the  relationship  of  the  supporting 
polygon,  in  this  case,  a  triangle,  to  the  CB  at  a  particular  instant  in  time  for  TRIPODO 
(LEGs  1,  3,  5)  and  TRIPODl  (LEGs  2,  4,  6).  The  CB  is  represented  by  the  point 
Pi  the  feet  are  represented  by  the  points  =  {x2,y2)>  P%  - 

P4  =(^4J>'4)  Point  P2  is  always  assigned  to  either  FOOTl  or  FOOT2,  depending  on 
whether  TRIPODO  of  TRIPODl,  respectively,  is  selected.  Point  p^  is  always  assigned  to 
either  FOOT3  or  FOOT4  and  point  p^  is  always  assigned  to  either  FOOTS  or  FOOT6, 
The  CB  and  feet,  then,  divide  the  support  polygon  into  three  smaller  triangles. 
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Kanayama  defines  three  modes  of  a  triple  of  points;  counterclockwise  (CCW), 
clockwise  (CW),  and  colinear  [KAN92].  A  counterclockwise  mode  results  when  the  order 
of  the  points  is  as  shown  in  Figure  5-2(a).  A  clockwise  mode  results  when  the  order  of  the 
points  is  as  shown  in  Figure  5-2(b).  With  these  definitions  in  mind,  we  can  describe  the 
area,  and  thereby,  the  stability,  of  a  triangle  as  a  triple  of  distinct  points  such  that 


SiPx.Pt.Pi) 


\ 

2 


1  1  1 

X,  Xj 

>'i  y2  >-3 


(5.1) 


Expanding  equation  5.1  yields: 

^(Pi ,P2,Pi)  =  ^[(^2  - )(>'3  - >^1 ) - (^3  - )(>'2  - >"1  )]•  (5  2) 

The  orientation  of  points  is  always  chosen  such  that: 

♦  if  the  CB  is  inside  the  supporting  polygon,  the  order  is  CCW  and  S  is  positive,  or 

♦  if  the  CB  is  outside  the  supporting  polygon,  the  order  is  CW  and  S  is  negative,  or 

♦  if  the  CB  is  on  the  supporting  polygon,  the  order  is  colinear  and  S  is  zero. 

If  5’  is  >  zero,  the  tripod  is  considered  stable.  Figuie  5-1  illustrates  a  statically  stable 
support  pattern.  If  5  <  zero,  the  tripod  is  considered  unstable.  Figure  5-3  illustrates  a 
statically  unstable  support  pattern.  The  safety  margin  of  12.5  centimeters  discussed  earlier 
is  not  implicitly  used  in  the  evaluation  of  stability.  The  Matlab  function  stable.m,  found  in 
Appendix  B,  performs  the  stability  test  described  above.  [KAN92] 

Once  the  stability  is  ascertained,  the  stability  intercepts  and  longitudinal  stability 
margin  5;  are  found.  The  stability  intercepts  are  determined  using  Kanayama's  method  for 
finding  the  intersection  of  two  directed  lines,  discussed  in  Chapter  IV.  The  stability  margin 
is  simply  the  distance  between  the  CB  coordinates  and  the  stability  intercepts  in  the 


72 


Figure  5-2(b).  Clockwise  Mode  of  a  Triple  of  Points  (After  [KAN92]). 


FOOD 


Figure  5-3.  Unstable  TRIPODs. 


direction  of  travel; 


S,  =  -  CbJ  (5.3) 

The  Matlab  function  stabint.m,  found  in  Appendix  B,  finds  the  stability  intercepts  of  a 
given  tripod  and  determines  the  stability  margin. 

C.  SUMMARY 

This  chapter  described  the  concept  of  static  stability  as  it  pertains  to  AquaRobot.  For 
simplification  purposes,  the  body  center  was  adopted  as  the  vehicle's  center  of  gravity.  A 
method  for  determining  static  stability,  including  the  longitudinal  stability  margin,  was 
derived.  The  following  chapter  describes  AquaRobot's  current  rectangular  leg  motion 
model  and  proposes  two  alternative  smooth  leg  motion  models;  elliptical  and  cycloidal. 
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VL  SMOOTH  FOOT  MOTION 


A.  INTRODUCTION 

Rough,  jagged  motion  tends  to  cause  abnormal  wear  of  m^hanical  mechanisms, 
vibrations  resulting  from  excited  mechanical  resonances,  and  inefficiencies  of  motor 
operation.  Therefore,  smooth  motion  is  a  desirable  characteristic.  A  smooth  motion 
function  is  one  which  is  continuous  and  which  has  a  continuous  first  derivative  and, 
possibly,  a  continuous  second  derivative.  Smooth  motion  for  AquaRobot  implies  smooth 
foot  and  smooth  body  motion.  This  chapter  is  concerned  with  developing  smooth  foot 
motion  models.  Smooth  foot  motion,  realized  through  smooth  foot  trajectories,  results  in 
decreased  wear  and  tear  on  motors,  gears,  and  joints  and  increased  motor  efficiency.  Here, 
trajectories  refer  to  the  time  history  of  position  and  velocity  for  the  three  degrees  of 
freedom  of  the  feet.  [CRA86] 

To  guarantee  smooth  trajectories  for  the  foot,  some  constraints  on  the  spatial  and 
temporal  qualities  between  footholds  is  required.  These  constraints  are  realized  through 
the  use  of  curved  trajectories.  There  are  many  curves  with  the  potential  to  provide  smooth 
leg  motion.  The  criteria  for  selecting  the  two  curves  used  here  are. 

♦  maintain  an  orientation  normal  to  the  terrmn  at  liftoff  and  touchdown,  and 

♦  be  continuous  through  the  second  derivative. 

B.  RECTANGULAR  FOOT  MOTION 

AquaRobot  presently  uses  a  rectangular  foot  motion.  Generally  speaking,  the  feet  are 
moved  upward,  forward,  downward,  and  backward  along  straight  lines  of  travel 
pWASSa].  Figure  6-1  illustrates  the  resulting  rectangular  foot  motion  generated  using  the 
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Figure  6-1.  Rectangular  Foot  Motion. 


existing  algorithm.  If  (x,>')  is  an  arbitrary  foot  point,  the  new  foot  point  is  determined 
using  {x+dx,y+dy).  The  junction  of  the  straight  lines  forming  the  rectangle  indicate 
points  where  motor  speed  control  is  discontinuous. 


C.  SMOOTH  FOOT  MOTION  -  ELLIPSE 

An  elliptical  foot  motion  was  chosen  as  the  primary  smooth  leg  motion  model  to 
implement.  The  ellipse  meets  both  criteria  established  in  the  introduction  to  the  chapter. 
Figure  6-2  illustrates  the  construction  and  result  of  an  elliptical  foot  motion  trajectory.  The 
parametric  representation  of  the  arbitrary  foot  position  (x,y)  begins  with 


x  =  acos0 
y  =  6sm0. 


(6.1) 


The  first  derivative  of  equations  6. 1  result  in 


—  =  -asin 
d6 


de 


=  bcosO. 


A  small  segment  ds  of  the  ellipse  is  then  calculated  using 


and  then 


The  new  foot  position  is  then  determined  using 

x^^acoie^^e) 

J«w  =  *sin(^+  6.9). 


(6.2) 


(6.3) 


(6.4) 


(6.5) 
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The  positive  trunor  axis  b  of  the  ellipse  is  constrained  such  that  the  fixed  CB  height  is 
retained  and  the  joint  limits  are  not  exceeded. 

D.  SMOOTH  FOOT  MOTION  -  CYCLOID 


A  cycloidal  foot  motion  was  chosen  as  the  alternate  smooth  leg  motion  model  to 
implement.  The  cycloid  meets  both  criteria  established  in  the  introduction  to  this  chapter. 
Figure  6-3  illustrates  the  construction  and  result  of  a  cycloidal  foot  motion  trajectory  The 
parametric  representation  of  the  arbitrary  foot  position  (x,>/)  begins  with 


x~a{0-  sin  0) 
y~b{l-cos^. 


(6.6) 


The  first  derivative  of  equations  6.6  result  in 


dx 

le 

de 


=  a(l-cos0) 
-  a  %m0. 


A  small  segment  ds  of  the  cycloid  is  then  calculated  using 


and  then 


(67) 


(6.8) 


(6.9) 


The  new  foot  position  is  then  determined  using 

x^  =  a[(  0+  A0)  -  sin(  0+  A6j)] 
3'«*=«[l-cos(0+A6')]. 


(6.5) 


80 


The  radius  a  of  the  circle,  of  which  a  point  on  its  circumference  traces  the  cycloid,  is 
constrained  such  that  the  fixed  CB  height  is  retained  and  the  joint  limits  are  not  exceeded. 
E.  SUMMARY 

In  this  chapter,  two  smooth  foot  motion  models  were  developed:  elliptical  and 
cycloidal.  The  next  chapter  describes  the  gait  planning  process  for  the  alternating  tripod 
gait  used  in  this  study. 
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VBL  GATT  PLANNING 


A.  INTRODUCTION 

"Leg  motion  planning  algorithms  are  designed  to  generate  the  foot  trajectory  for  the 
support  and  transfer  phases  and  to  choose  the  desired  footholds  for  the  transfer  legs  based 
on  the  body  velocity  and  the  constrained  working  volume  (CWV)"  [LEE88a].  Leg  motion 
planning  is  sometimes  decomposed  into  separate  gait  selection  and  gait  implementation 
tasks.  Gait  selection  is  generally  regarded  as  the  prior  decision  to  use,  or  select,  a  specific 
gait.  This  study  concentrates  on  the  alternating  tripod  gait  because  it  allows  reasonably 
fast  walking,  yet  maintains  static  stability  at  all  times.  Gait  implementation,  then,  is  the 
execution  of  the  chosen  gait  through  foothold  selection  and  joint  angle  determination. 
Because  there  is  so  much  interaction  between  the  gait  selection  and  gait  implementation 
tasks,  we  choose  to  combine  them  into  one  task  termed  gait  planning. 

B.  RECTANGULAR  TRIPOD  GAIT 

The  fundamental  walking  algorithm  for  AquaRobot  is  an  up,  over,  and  down  motion 
of  a  group  of  three  legs.  As  in  previous  analyses,  we  call  the  first  group  of  legs  TRIPODO 
(LEGs  1,3,5)  with  body-fixed  coordinates  {x3,y1,zi),  and  (x5,>'5,z5), 

respectively.  Similarly,  we  call  the  second  group  of  legs  TRIPOD  1  (LEGs  2,4,6)  with 
body-fixed  coordinates  (x2,y2,z2),  (x4,j4,z4),  and  (x6,_y6,z6).  We  begin  Avith  all  legs 

touching  the  ground.  A  direction  for  the  robot  body  to  travel  is  selected.  Then  the  distance 
for  the  robot  body  to  move  is  determined,  possibly  based  on  a  desired  goal  point.  From 
this  information,  the  new  foot  positions  are  chosen.  Then,  the  body-fixed  coordinates  of  a 
selected  group  of  legs  are  changed  by  the  difference  between  the  existing  foot  positions 
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and  the  new  foot  positions.  The  LEGs  in  a  group  are  then  conunanded  to  move  in  the 
x-axis  direction,  (fy  in  the  y-axis  direction,  and  dz  in  the  z-axis  direction: 


(xi,^!,^!)  =  {x\-\-dx,y\-k-dy,z\-^dz). 

(7.1) 

(x3,>'3,23)  =  (x3+<&,_y3+£^,23+tir),  and 

(7.2) 

(x5,  yS,  zS)  =  {xS+dx,yS+cfy,zS+dz). 

(7.3) 

TRIPODO  is  generally  selected  as  the  first  group  of  legs  to  move,  although  this  is  a  purely 
arbitrary  decision.  Once  the  TRIPODO  feet  are  free  fi'om  the  ground,  the  robot  body  is 
moved  along  the  desired  direction  of  travel.  When  the  TRIPODO  feet  are  firmly  planted, 
calculations  are  begun  to  move  TRIPOD  1,  using  the  commanded  motion  derived  fi-om; 


(x2,>’2,z2)  =  (x2  +dx,yl+(fy,z2->i-dz), 

(7.4) 

{xA,yA,z4,)  =  {x4+dx,yA+dy^zA->rdz),  and 

(7.5) 

{x6,y6,z6)  =  {x6+dx,y6-^dy,z6-¥dz). 

(7.6) 

Alternating  the  groups  of  legs,  while  choosing  the  same  direction,  causes  AquaRobot  to 
travel  along  a  straight  line  with  the  body  height  held  constant.  [IWA88a] 

We  choose  to  call  this  gait  the  Rectangular  Alternating  Tripod  Gait  (RATG) 
because  the  free  leg  trajectories  describe  a  rectangular  pattern.  Figure  7-1  illustrates  a 
representative  leg  motion  for  the  RATG.  First,  the  three  legs  of  the  free  tripod  (in  this 
case,  TRIPODO)  are  lifted  perpendicular  to  the  terrain  until  KNEE  joint  limits  are  reached. 
Essentially,  the  free  legs  are  lifted  straight  upward  until  the  feet  are  at  their  maximum 
height.  Next,  the  body  is  moved  in  the  direction  of  travel  until  HIP  joint  limits  are  reached. 
Then  the  free  legs  are  moved  in  the  direction  of  travel,  horizontal  with  the  terrain,  until  the 
free  feet  are  over  the  desired  touchdown  positions.  Finally,  the  fi-ee  legs  are  lowered 


Figure  7-1.  Rectangular  Tripod  Gait  Leg  Motion. 
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perpendicular  to  the  terrain  until  contact  sensors  in  each  foot  indicate  that  the  leg  motion 
is  completed. 

Gaits  can  be  expressed  as  functions  of  distance  or  time  in  gait  diagrams  [TOD86]. 
Figure  7-2  illustrates  the  distance  traveled  at  ten  successive  time  intervals,  /O  through  /9, 
when  a  RATG  is  used.  A  typical  sequence  of  events  for  the  RATG  is: 

♦  body  and  leg  components  in  START  posture,  (/O), 

♦  free  and  lift  the  legs  of  TRIPODO  (LEGs  1,  3,  5),  (/I), 

♦  move  the  CB  in  the  direction  of  travel,  (tl\ 

♦  move  the  free  legs  of  TRIPODO  in  the  direction  of  travel,  (r3), 

♦  place  the  legs  of  TRIPODO,  (tA), 

♦  free  and  lift  the  legs  of  TRIPOD  1  (LEGs  2,  4,  6),  (/5), 

♦  move  the  CB  in  the  direction  of  travel,  (/6), 

♦  move  the  free  legs  of  TRIPOD  1  in  the  direction  of  travel,  (tl), 

♦  place  the  legs  of  TRIPOD  1,  (/8),  and  then  the  process  repeats,  (/9). 

In  Figure  7-2,  a  solid  dot  (•)  indicates  the  foot  is  touching  the  terrain  and  an  empty  dot  (o) 
indicates  the  foot  is  free  from  the  terrain.  A  cross  (+)  indicates  the  position  of  the  robot's 
CB. 

Figure  7-3  shows  the  RATG  as  a  function  of  time.  The  motion  of  each  major  body 
component  (TRIPODO,  CB,  TRIPODl)  is  illustrated  using  a  tuning  diagram.  In  Figure  7- 
3,  a  solid  bar  (— )  along  the  upper  line  represents  movement  of  the  associated  body 
component  with  respect  to  the  ground.  A  solid  bar  along  the  lower  line  indicates  no 
movement  of  that  particular  body  component  -with  respect  to  the  ground.  Analysis  of 
Figure  7-3  reveals  several  times  when  body  components  are  not  moving.  For  example, 
during  time  neither  the  CB  nor  TRIPODl  are  moving.  These  areas  indicate  times  when 
joint  motors  are  not  powered.  Additionally,  the  CB  moves  during  only  one  time  interval 
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Rectangular  Gait  as  a  Function  of  Distance. 


(t2  or  t6)  for  each  TRIPOD  cycle.  These  periods  of  discontinuous  motion  are  ineflBcient, 
resulting  in  jerky  body  movement  and  slower  over-the-ground  speed. 

C.  DISCRETE  TRIPOD  GAIT 

The  Discrete  Alternating  Tripod  Gait  (DATG)  was  developed  as  the  natural  next 
step  after  the  rectangular  gait.  For  our  purposes,  non-continuous  motion  of  the  CB  is 
known  as  discrete  motion.  The  DATG  provides  for  discrete  motion  of  AquaRobot's  CB. 
The  distance  the  CB  travels  is  constrained  by  workspace  and  stability  limits.  For  ease  of 
calculation,  the  three  feet  in  a  tripod  describe  a  regular  triangle.  This  approach  does  not 
necessarily  lead  to  the  maximum  possible  stride  or  the  minimum  time  to  cover  a  distance. 
Nevertheless,  stability  is  easily  assured.  The  DATG  includes  an  elliptical  foot  motion 
trajectory  as  a  means  of  alloA^g  continuous  foot  motion  between  ground  contact  points. 
A  typical  sequence  of  events  for  the  DATG  is: 

♦  body  and  leg  components  in  START  posture,  (tO), 

♦  plan  an  elliptical  path  for  and  move  TRJPODO  to  its  destination,  (tl), 

♦  move  the  CB  in  the  direction  of  travel  until  a  limit  is  reached,  (t2), 

♦  plan  an  elliptical  path  for  and  move  TRIPOD  1  to  its  destination,  (G), 

♦  move  the  CB  in  the  direction  of  travel  until  a  limit  is  reached,  (t4),  and 

♦  continue  repeating  the  process,  (t5  through  t9). 

Figure  7-4  shows  the  DATG  as  a  function  of  distance.  Figure  7-5  shows  the  DATG 
as  a  function  of  time.  The  major  difference  between  the  rectangular  and  discrete  gaits  is 
the  leg  motion.  Except  for  periods  when  the  feet  are  leaving  or  contacting  the  terrain,  or 
when  only  the  CB  is  moving,  the  feet  are  in  continuous  motion.  The  leg  joint  motors  for 
the  free  legs  in  the  DATG,  then,  are  operating  almost  continuously.  However,  the  CB 
motion  is  discontinuous  in  both  gaits. 
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D.  CONTINUOUS  TRIPOD  GAIT 


The  Continuous  Alternating  Tripod  Gait  (CATG)  was  designed  to  allow  continuous 
motion  of  the  CB,  independent  of  the  grouped  leg  (tripod)  motions.  The  sequence  of 
events  is  similar  to  the  DATG,  except  the  CB  moves  continuously.  TRIPODs  zero  (0)  and 
one  (1)  still  alternate  and  the  foot  trajectories  are  still  elliptical;  however,  the  CB  is  not 
allowed  to  halt  once  the  walking  begins  (unless  the  goal  point  is  reached).  The  supporting 
tripod  provides  the  motion  for  the  CB. 

The  CB  motion  is  synchronized  with  the  motion  of  the  tripods.  The  desired  foot 
positions  are  used  to  generate  an  elliptical  trajectory  for  the  selected  free  tripod  in  the 
transfer  phase.  Then  the  distance  between  the  existing  and  desired  foot  positions  is  divided 
into  small  segments.  The  length  of  these  segments  is  then  used  to  determine  the  motion 
the  CB  makes  in  the  direction  of  travel  in  the  xy  plane.  The  body  height  is  held  constant, 
so  the  CB  is  not  moved  in  the  z  plane.  For  every  increment  of  free  tripod  foot  motion  in 
the  xy  plane,  the  supponing  tripod  increments  the  CB  motion  so  the  CB  completes  one- 
half  the  distance  the  free  tripod  travels  in  the  same  time  period.  The  CB  moves  one-half 
the  foot  distance  because  of  workspace  restrictions.  Actually,  the  supporting  tripod  joints 
are  moving,  thereby  moving  the  CB  indirectly.  The  increments  are  chosen  arbitrarily  small, 
in  our  case  100,  so  the  CB  appears  to  move  continuously.  The  CB  moves  in  the  direction 
of  travel  during  each  tripod  supporting  phase  and  when  all  six  legs  are  on  the  ground. 

From  observation  of  the  CATG,  the  maximum  possible  stride  is  146.04  centimeters. 
Therefore,  the  maximum  possible  CB  motion  is  73.02  centimeters.  The  AquaRobot 
simulation  can  be  run  with  a  default  maximum  stride  value  slightly  less  than  146 
centimeters  to  compensate  for  the  edges  of  the  footpads  touching  at  the  extreme  limits  of 
the  workspace. 


A  typical  sequence  of  events  for  the  CATG  follows.  No  time  steps  are  given  because 
all  major  body  components  are  continuously  in  motion.  Initially,  the  body  and  leg 
components  are  in  the  START  posture.  The  desired  foot  positions  for  each  foot  in  one 
tripod  are  determined  using  workspace  and  stability  constraints.  The  stride  chosen  is  the 
minimum  distance  any  one  of  the  three  feet  in  a  tripod  grouping  can  move  in  its  individual 
workspace.  TRIPODO  is  arbitrarily  selected  and  an  elliptical  path  is  planned  to  move  its 
feet  one  increment  towards  their  new  destination.  For  the  first  step,  the  CB  is  moved  a 
distance  equal  to  the  maximum  stride.  Therefore,  then  new  location  for  the  CB  to  travel  to 
is  calculated  using 

CB(x^)  =  CB(x^)  +  {stride)  *cos{direction),  and  (7.7) 

CB(y^)  =  CB{y^)  +  [stride)  *  %\n{direction).  (7.8) 

TRIPOD  1  then  moves  the  CB  the  distance  required  to  attain  the  new  CB 
coordinates.  For  the  next  steps,  the  CB  is  moved  a  distance  equal  to  one-half  the 
maximum  stride.  In  this  case,  the  new  location  for  the  CB  to  travel  to  is  calculated  using 

CB{x^)  =  CB(x„)  +  |^^^^j*cos(fil/rec//(?n),  and  (7.9) 

CB(y^)  =  C5Ck,,) sin(r/;recnow).  (7. 10) 

After  each  incrementation  of  the  CB  position,  its  location  is  compared  to  the  goal  point.  If 
the  distance  from  the  CB  to  the  goal  point  is  less  than  would  result  if  the  maximum  stride 
were  used,  the  lessor  distance  is  used.  Then,  the  new  stride  length  becomes  twice  the 
d'  stance  from  the  CB  to  the  goal  point. 

Figure  7-6  shows  the  CATG  as  a  function  of  distance.  The  motion  of  the  legs  is  the 
same  as  the  DATG.  Figure  7-7  shows  the  CATG  as  a  function  of  time.  This  figure 
illustrates  the  continuous  motion  of  the  major  body  components. 
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E.  SUMMARY 


In  this  chapter,  the  gait  planning  algorithms  for  the  rectangular,  discrete,  and 
continuous  alternating  tripod  gaits  were  described.  The  next  chapter  implements  the 
discrete  and  continuous  gait  planning  algorithms  in  computer  code. 
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Vm.  AQUAROBOTSJMULATOR 


A.  INTRODUCTION 

In  this  chapter,  the  AquaRobot  Simulator  program  is  presented.  Initially,  the 
simulation  facilities,  tools,  and  methods  are  described.  Then  the  internal  structure  and  flow 
of  the  program  is  discussed.  One  part  of  the  program  is  a  three-dimensional  (3D)  stick- 
figure  simulation  of  the  DATG.  Another  part  of  the  program  is  a  3D  stick-figure 
simulation  of  the  CATG.  A  complete  listing  of  program  code  is  found  in  Appendix  C.  A 
separable  User's  Guide,  with  instructions  on  how  to  use  the  program,  is  included  as 
Appendix  D. 

B.  SIMULATION  FACBLITIES 

A  critical  part  of  this  thesis  included  integrating  the  gait  planning  module  design  with 
a  previously  developed  graphics  simulation  program  [DAV93].  As  a  result,  some  graphics 
code  required  revision  or  augmentation.  Additionally,  the  gait  planning  code  modules 
required  some  transportability  between  the  AquaRobot  control  computer  (NEC  486-based 
Personal  Computer  with  MS-DOS  operating  system)  and  the  AquaRobot  Simulator 
(Silicon  Graphics  Personal  Iris  Workstation).  To  ensure  some  level  of  transportability 
between  the  personal  computer  and  the  workstation,  the  ANSI  standard  C  format  was 
adopted.  The  graphics  code  modules  are  specific  to  the  Silicon  Graphics  Workstation. 

Many  portions  of  the  gait  planning  algorithms  were  originally  written  and  tested  in 
Matlab.  a  "C"-based  development  language.  Appendk  B  contains  the  Matlab  code.  Once 
the  basic  algorithms  were  verified,  they  were  converted  to  the  language  for 
implementation  in  the  AquaRobot  Simulator.  The  C"^  language  was  used  for  grapWcs,  for 
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interfacing  between  the  gait  planning  and  graphics  modules,  and  whenever  specific  objects 
(legs,  body,  etc.)  could  be  defined.  Appendix  C  contains  C**  code. 


C  MODULE  STRUCTURE 

Figure  8-1  contains  a  flow  chart  of  the  AquaRobot  Simulator  program.  The 
simulation  is  essentially  one  program  with  options  for  either  the  DATG  or  the  CATG. 
Typing  aqua  starts  the  simulation.  The  user  then  determines  the  distance,  and  thereby  the 
direction,  the  simulated  AquaRobot  travels  at  the  outset  of  the  program;  the  goal  point,  in 
(x,^)  coordinates,  is  entered  from  the  keyboard.  Any  arbitrary  distance  (direction)  is 

acceptable.  Once  the  graphics  portion  of  the  program  begins,  the  user  can  select  the 
camera  viewing  angle.  When  the  simulated  AquaRobot  reaches  the  desired  goal  point,  it 
stops.  A  description  of  each  functional  module  follows. 

The  simulator  files  are  separated  into  four  modules.  The  files  that  comprise  the 
Makefile,  Graphics,  and  Matrix  Manipulation  Modules  are  only  cursorily  addressed  here. 
The  Gait  Planning  Module  is  described  in  more  detail. 

1.  Makefile 

The  Makefile  allows  the  make  utility  to  intelligently  compile  the  program.  This 
file  includes  instructions  for  how  and  when  to  compile  the  various  files  that  comprise  the 
AquaRobot  Simulator.  The  AquaRobot  simulation  program  uses  the  UNIX  make  utility  to 
link  together  the  27  individual  files  with  the  graphics,  math,  and  standard  input-output 
libraries.  In  this  case,  the  Makefile  generates  the  executable  program  aqua. 

1.  Graphics 

The  3D  stick-figure  graphics  code  was  originally  written  by  Sandra  Davidson 
[DAV93].  The  graphics  files  (code  modules)  are  included  in  Appendix  C  for  easier 
reference.  Some  file  names  were  changed  to  avoid  software  configuration  management 
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Figure  8-1.  AquaRobot  Simulator  Flow  Chart. 
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problems.  Additionally,  some  graphics  code  was  modified  because  of  the  requirements  of 
the  gait  planning  code.  For  example,  the  FARCLIPPING  value  and  the  CAMERA  viewing 
angles  were  changed  in  the  bot.H  file  and  the  CB  height  was  changed  in  the  AbBody.C 
file.  Because  the  Graphics  Module  polls  the  Gait  Planning  Module,  a  suitable  interface 
between  the  two  was  constructed  in  the  Kinematics.H  and  Kinematics.C  files  The 
graphics  code  consists  of  the  following  files; 

♦  AbBody.H  and  AbBody.  C, 

♦  AbLeg.H  and  AbLeg.  C, 

♦  AbRigidH  and  AbRigid  C; 

♦  bot.H  and  bot.C, 

♦  Kinematics-H  and  Kinematics.  C, 

♦  LinkH  and  Link. C; 

«  LinkO.H  and  LinkO.  C; 

♦  Linkl.H  and  Linkl.  C; 

♦  Linkl.H  and  Linkl. C\  and 

♦  Links. H  and  Links.  C. 

3.  Matrix  Manipulation 

The  matrix  manipulation  code  was  also  originally  written  by  Sandra  Davidson 
[DAV93].  It  provides  4x4  matrix  multiplication  capability.  This  code  is  an  essential  part 
of  the  graphics  code,  although  it  is  not  used  in  the  gait  planning  code.  The  Matrix 
Manipulation  Module  includes  the  following  files,  found  in  Appendix  C  . 

♦  MatrixMy.H  and  MatrixMy.  C. 
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4.  Gait  Planning 

The  Gait  Planning  Module  (GPM)  consists  of  the  following  files: 

♦  arconst.H; 

♦  cajunc.H  and  arfunc.  C;  and 

♦  artpgait.H  and  artpgait  C. 

To  support  easier  reference  and  nraintenance,  most  constants  are  grouped  together  into 
the  file  arconst.H  (aquarobot  constants). 

The  arfunc.H/C  (aquarobot  functions)  files  contain  13  functions.  The  min  and 
max  functions  simply  return  the  minimum  or  maximum  of  two  expressions,  respectively. 
The  ellipse  function  calculates  the  incremental  foot  trajectory  along  an  elliptical  path 
between  the  current  foot  position  and  the  desired  foot  position.  The  kinematics  function 
performs  kinematics  for  the  Gait  Planning  Module.  Kinematics  for  the  Graphics  Module 
are  performed  in  the  Graphics  Kinematics.H/C  files.  The  inv  kinematics  function 
performs  the  inverse  kinematics  operations.  The  world  hody  function  provides  coordinate 
transformation  fi-om  the  body-fixed  coordinate  system  to  the  world  coordinate  system. 
The  bodyjworld  function  provides  coordinate  transformation  from  the  world  coordinate 
system  to  the  body-fixed  coordinate  system. 

The  maxdistance_25cm  function  determines  the  maximum  stride  for  the 
workspace  defined  by  the  25  centimeter  footpad.  The  maxdistance_45cm  function 
determines  the  maximum  stride  for  the  workspace  defined  by  the  45  centimeter  footpad. 
The  arcint  function  calculates  the  intersection  of  a  directed  line  and  an  arc  segment.  The 
segint  function  calculates  the  intersection  of  a  directed  line  and  a  line  segment.  The  stable 
function  calculates  the  stability  of  a  tripod.  The  staparam  function  determines  the 
longitudinal  stability  margin  and  the  x  mdy  intercepts  of  the  directional  ray  originating  at 
the  CB  and  terminating  at  the  point  of  intersection  on  the  selected  tripod. 
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The  artpgait.H/C  (aquarobot  tripod  gait)  files  15  functions.  The  get^oal 
function  allows  the  user  to  input  the  desired  goal  point  to  which  the  AquaRobot  simulation 
walks.  The  getjnerm  function  allows  the  user  to  choose  between  the  DATG  or  the 
CATG.  The  get Jdotchoice  function  allows  the  user  to  determine  whether  the  25 
centimeter  or  45  centimeter  footpad  is  used.  The  gait  algorithm  function  executes  either 
the  DATG  or  the  CATG  gait  algorithms,  as  previously  determined  by  the  user.  The 
robot  model  function  initializes  the  joint  angles  for  the  simulation. 

The  disc  setjlag  function  initializes  the  flags  used  during  the  DATG  algorithm. 
The  disc_gait _planning  function  determines  the  walking  parameters,  such  as  stride  length 
and  direction,  used  during  the  DATG  algorithm.  The  disc  tripod  motion  function 
calculates  and  executes  incremental  joint  motions  necessary  to  move  the  AquaRobot 
simulation  using  the  DATG. 

The  cant  set  Jlag  function  initializes  the  flags  used  during  the  CATG  algorithm. 
The  contjgait  j)lanning  function  determines  the  walking  parameters,  such  as  stride  length 
and  direction,  used  during  the  CATG  algorithm.  The  cont  tripod  motion  function 
calculates  and  executes  the  incremental  joint  motions  necessary  to  move  the  AquaRobot 
simulation  using  the  CATG. 

Some  functions  in  the  artpgait.C  file  are  used  only  for  printing  out  the  status  of 
joints,  feet,  etc.  These  functions  include  print  status,  print jwalkpara,  print _grm_data, 
print  Joint  data. 

D.  SUMMARY 

This  chapter  described  the  AquaRobot  simulation  program.  A  discussion  of  the 
simulation  facilities,  tools,  and  methods  followed.  Then,  the  organization  of  the  program 
was  described.  The  final  chapter  contains  results  and  concluding  remarks. 
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K.  CONCLUSIONS 


A.  RESULTS 

Both  the  DATG  and  CATG  algorithms  work  successfully.  Most  of  the  coding  is 
shared  between  the  two  algorithms.  The  Gait  Planning  Module,  of  which  the  DATG  and 
CATG  are  a  part,  requires  approximately  1600  total  lines  of  code.  Graphics  code  is 
excluded  from  the  line  count. 

Figures  9- 1(a),  9- 1(b),  and  9- 1(c)  juxtapose  the  diagrams  for  the  rectangular, 
discrete,  and  continuous  alternating  tripod  gaits.  It  is  obvious  from  the  figures  that  there  is 
a  progression  of  improvement  in  continuous  CB  motion  fro  the  RATG  to  the  CATG. 

Many  areas  of  the  RATG  and  DATG  gait  diagrams  show  no  major  body  component 
(TRIPODO,  CB,  TRIPOD  1)  motion.  These  areas  indicate  times  when  joint  motors  are 
operating  discontinuously,  i.e.,  motors  are  stopped.  Starting  and  stopping  the  joint  motors 
results  in  rough,  jagged  motion.  This,  in  turn,  tends  to  cause  abnormal  wear  of  mechanical 
mechanisms,  vibrations  resulting  from  excited  mechanical  resonances,  and  inefGciencies  of 
motor  operation.  Smoother,  more  continuous  motion  results  in  decreased  wear  and  tear 
on  motors,  gears,  and  joints  and  increased  motor  efficiency.  The  CATG,  with  elliptical 
foot  trajectory,  allows  these  benefits  to  occur. 

Figures  9-2  through  9-7  present  the  motion  of  the  individual  leg  joints  (HIP,  KNEEl, 
and  KNEE2)  for  the  first  few  tripod  cycles  of  the  DATG  and  the  CATG  when  walking 
with  LEGl  along  the  x-axis.  The  upper  plot  in  each  figure  (a)  shows  the  DATG;  the  lower 
plot  in  each  figure  (b)  shows  the  CATG.  As  seen  in  Figures  9-2  and  9-5,  there  is  no  HIP 
motion  for  the  legs  of  the  tripod  that  lie  on  the  x-axis:  LEGl  in  TRIPODO  and  LEG4  in 
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to  tl  t2  t3  t4  t5  t6  t7  t8  t9 

Figure  9-l(a).  Rectangular  Alternating  Tripod  Gait. 


to  tl  t2  t3  t4  t5  t6  t7  t8  t9 

Figure  9-l(b).  Discrete  Alternating  Tripod  Gait. 
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Figure  9-l(c).  Continuous  Alternating  Tripod  Gait. 


Time  -  Samples 


Time  -  Samples 


Time  -  Samples 


Figure  9-7(a).  LEG6  Joint  Angle  Motion  for  DATG 


Time  -  Samples 

Figure  9-7(b).  LEG6  Joint  Angle  Motion  for  CATG, 


TRIPOD  1.  A  comparison  of  joint  angle  motion  for  the  two  gait  algorithms  indicates; 

♦  there  are  periods  of  no  joint  motion  in  the  discrete  gait, 

♦  there  are  no  such  periods  in  the  continuous  gait,  and 

♦  in  both  gaits,  there  are  discontinuities  when  the  feet  touch  the  ground. 

The  goals  of  this  thesis,  outlined  in  Chapter  in,  were  to  provide  simulation  of  an 
improved  alternating  tripod  gait  that  included; 

♦  smooth  leg  motion, 

«  omnidirectional  body  movement, 

♦  continuous  body  motion,  and 

♦  limited  path  following  capability. 

These  goals  have  been  met  en  route  to  the  completion  of  this  thesis. 

B.  RESEARCH  CONTRIBUTIONS 

This  thesis  is  a  direct  outgrowth  of  a  cooperative  research  effort  between  the  Naval 
Postgraduate  School  and  the  Japanese  Port  and  Harbor  Research  Institute  to  enhance  the 
hardware  and  software  capabilities  of  the  AquaRobot  underwater  walking  machine. 

The  PHRI  originally  developed  AquaRobot  to  replace  hard-hat  divers  constructing 
tsunami  barriers  in  the  hazardous  underwater  environment  off  Japan's  coast.  Although 
AquaRobot  has  demonstrated  significant  ability  as  the  first  hexapod  underwater  walking 
machine,  it  is  still  unable  to  perform  its  designed  task  more  efficiently  or  less  costly  than 
human  divers.  AquaRobot  speed  and  agility  enhancements  derived  from  this  thesis  will 
result  in  a  more  eflScient  and  less  costly  machine.  This  translates  directly  into  reduced  risk 
of  human  injury  and  decreased  construction  costs. 

Although  walking  machine  research  and  construction  is  extensive,  AquaRobot  is  the 
first  walking  robot  with  a  practical  use.  As  such,  improvements  to  AquaRobot  directly 


result  in  concept  fulfillment  and  favorably  demonstrate  the  usefulness  of  robots,  and 
walking  robots  in  particular. 

This  opportunity  to  work  and  study  with  the  PHRI  lays  the  foundation  for  future 
cooperative  U.S.- Japanese  research  activities  in  robotics.  Even  marginal  headway  towards 
improving  U.S.- Japanese  electronics  and  robotics  research  efforts  will  represent  a 
significant  cooperation  milestone  reached. 

C.  RESEARCH  EXTENSIONS 
1.  Required  Future  Work 

Towards  the  end  of  the  thesis  research,  an  opportunity  to  visit  the  PHRI  in 
Japan  presented  itself  [PHR93].  During  the  week-long  visit,  three  sigiuficant  dimensional 
changes  to  the  AquaRobot  were  discovered  that  affect  the  workspace,  stability,  and 
maximum  stride  results  in  this  thesis.  The  dimensional  changes  to  AquaRobot  were  the 
result  of  ongoing  research  at  the  PHRI  into  the  machine's  mechanical  attributes.  None  of 
the  changes  impact  the  derivation  of  the  kinematics,  inverse  kinematics,  stability,  stride,  or 
gait  algorithms.  However,  the  numerical  values  used  in  the  calculations  are  slightly 
different  from  those  culled  fi'om  papers  and  blueprints.  The  three  changes  were: 

♦  the  footpad  diameter  increased  from  25  centimeters  to  45  centimeters, 

♦  the  footpad  angle  decreased  fi’om  ±  45®  to  ±  22.5°,  and 

♦  the  lengths  of  LINK2  and  LINK3  increased  1.8  and  1.0  centimeters,  respectively. 

The  LINK  dimension  changes  and  the  footpad  angle  change  are  reflected  in 
Table  9-1.  LINKO  and  LINKl  were  not  changed.  Figure  9-8  shows  the  reduced 
workspace  resulting  fi'om  the  dimensional  changes.  The  original  workspace  for  LEGl  is 
shown  with  dashed  lines  to  emphasize  the  change  in  working  area. 
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The  dimensional  changes  affect  the  workspace,  stability,  and  maximum  stride. 
First,  the  new  workspace  is  reduced  from  its  previous  size.  However,  the  workspace  is 
used  in  the  same  manner  as  before.  Next,  stability  is  increased  because  the  radius  of  the 
foot  pad  almost  doubled.  Because  stability  is  measured  at  the  center  of  the  footpad,  the 


additional  radius  actually  imparts  a  greater  stability  safety  margin.  Finally,  because  of  the 
footpad  angle  change,  the  maximum  stride  length  is  reduced  from  178.21  centimeters  to 
147.57  centimeters.  The  total  workspace  area  is  reduced  from  10367  to  5509  square 
centimeters,  or  approximately  one-half  its  previous  size. 


Table  9-i.  New  Link  Parameters  yorAquaRobot. 
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0 
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e. 
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2 
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e. 
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3 

0 

0 

9, 

1  4 

0 

^3 

0 

9. 

a.  Unconstrained  Workspace 

This  thesis  was  developed  using  a  constrained  workspace.  The  workspace 
must  eventually  be  unconstrained  to  determine  which  gaits,  without  sacrificing  vehicle 
stability  and  energy  efficiency  and  while  maintaining  continuous  body  motion,  result  in  the 
fastest  speeds  for  a  hexapod  underwater  walking  robot.  This  future  work  should  primarily 
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Figure  9-8.  New  Constrained  Horizontal  Workspace. 


be  directed  towards  investigating  whether  there  is  an  analytical  method  for  optimizing  the 
leg  cycling  of  a  walking  vehicle  for  an  arbitrary  given  maneuvering  specification  while 
moving  over  rough  terrain.  Successful  realization  of  this  research  would  result  in  on-line 
optimization  of  foot  placement. 

b.  Six  DOF  Body  Movement 

Statically  stable  alternating  tripod  gaits  were  planned  and  implemented  in  this  thesis.  To 
ease  imtiai  gait  implementation,  flat  terrain,  fixed  body  height,  fixed  body  orientation,  and 
straight  line  path  constraints  were  placed  on  the  vehicle.  Constraints  must  gradually  be 
removed  to  allow  rough-terrain  walking  capability  with  an  arbitrary  orientation.  Future 
work  should  include  lifting  the  constraints  on  body  movement:  allow  the  body  orientation 
and  body  height  to  change.  If  these  efforts  are  completed,  the  AquaRobot  simulation  can 
then  include  navigation  over  irregular  terrain. 

c.  Dynamic  Effects 

A  study  of  the  effects  of  the  aqueous  medium,  ocean  currents,  tether,  and 
camera  boom  loading  on  AquaRobot  is  advised.  The  greatest  limiting  effect  on 
AquaRobot's  speed  may  be  the  effects  of  the  aqueous  medium,  including  the  effects  of 
underwater  currents,  on  the  movement  of  the  body  and  the  legs.  Environmental  effects 
include  the  salt  water  medium  itself  and  the  approximately  two  knot  subsurface  ocean 
current  typical  off  Japan's  coast.  Non-ideal  characteristics  include  the  effects  of  the  tether 
and  the  main  body  camera  boom  on  efficient  walking. 

2.  Potential  Future  Work 

The  following  paragraphs  describe  potential  future  work.  The  work  is  potential 
because  research  has  shown  that  alternating  tripod  gaits  are  a  simple  way  to  achieve 
reasonable  walking  speed  while  maintaining  static  stability. 


a.  Other  Stable  Gaits 

This  thesis  research  concentrated  on  statically  stable  alternating  tripod 
gaits.  Additionally,  only  equilateral  triangles  (tripods)  were  considered.  Considering 
isosceles,  or  other  irregularly  shaped,  triangles  may  result  in  a  more  eflScient  use  of  stride 
and  stability  parameters  towards  increasing  vehicle  speed.  Furthermore,  because 
AquaRobot's  six  legs  are  located  at  60  degree  intervals,  at  least  two  bilateral  symmetries 
are  attainable.  Symmetry  is  attained  when  a  fore  and  aft  pair  of  legs  is  defined  or  when  any 
three  legs  are  distinguished  from  the  other  three  legs.  One  potentially  important  question  is 
whether  these  alternative  symmetries  can  be  exploited  to  increase  overall  speed  when 
arbitrary  vehicle  motion  is  desired. 

b.  Unstable  (Free)  Gaits 

A  potential  future  research  topic  includes  determining  whether  there  are 
optimality  improvements  when  gaits  are  selected  that  result  in  momentary  vehicle 
instability.  The  important  question  here  is  whether  the  viscous  effects  of  the  aqueous 
medium  (especially  the  natural  buoyancy  of  salt  water)  allows  gaits  which  would  be 
unstable  in  air  to  be  used  in  the  water. 
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APPENDIX  A: 


OVERVIEW  AND  fflSTORY 


AquaRobot  was  conceived  and  designed  at  the  Japanese  Port  and  Harbor  Research 
Institute  (PHRI)  in  1984.  Its  main  purpose  is  to  replace  divers  in  carrying  out  underwater 
inspection  work  associated  with  various  port  construction  projects.  Specifically,  the 
AquaRobot  project  is  an  outgrowth  of  a  multi-billion  dollar  project  to  construct  a  tsunami 
barrier  at  the  entrance  to  Japan's  Kamaishi  Bay.  The  primary  design  function  of 
AquaRobot  is  to  measure  the  flatness  of  underwater  rubble  mounds  used  as  foundations  to 
support  concrete  caissons  that  form  the  tsunami  barrier.  A  secondary  design  function  is  to 
provide  for  observation  of  the  underwat  ;r  construction  effort  using  a  television  camera. 

Figure  A-1  shows  a  representation  of  the  expected  underwater  operation  of 
AquaRobot.  Navigational  accuracy  of  AquaRobot  is  attained  using  a  transponder  system 
that  allows  position  errors  of  less  than  ten  centimeters  at  a  distance  of  300  meters 
[AKI89].  Every  two  steps,  AquaRobot  receives  an  updated  navigational  position  from  the 
transponder  system  [IWA90].  AquaRobot  is  tethered  to  a  control  platform,  in  this  case  a 
ship,  vwth  a  100  meter  cable  that  includes  optical  fiber  and  metal  wire  links  [IWA88b]. 

Three  models  of  AquaRobot  have  been  constructed  to  date.  The  first  model, 
completed  in  1985,  was  an  experimental  version  developed  for  concept  exploration  and 
validation.  It  is  used  solely  for  overground  testing.  The  experimental  model  weighs  only 
280  kilograms,  yet  can  cany  one  person  on  its  back.  The  second  model,  constructed  in 
1987,  is  the  prototype  version.  The  prototype  is  almost  twice  as  large  as  the  experimental 
model,  mainly  because  of  the  increased  leg  length  and  the  addition  of  the  camera  boom 
assembly,  tether  linkage,  and  lifting  apparatus.  The  prototype  is  watertight  to  50  meters 
depth,  thereby  causing  additional  bulk.  The  prototype  weighs  700  kilograms.  The  third 
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Figure  A-1.  Underwater  Operation  of  AquaRobot  (After  [IWA88a]). 


ns 


model,  finished  in  1989,  is  a  light  weight  version  of  the  prototype.  Its  improvements  over 
the  prototype  include  approximately  half  the  weight  and  cable  runs  made  inside  leg 
articulations.  [IWA90] 

The  body  of  AquaRobot  is  constructed  of  anti-corrosive  alununum  and  is  hexagonal 
in  shape.  Sk  legs  are  attached  at  60  degree  increments  about  the  body.  Legs  are  numbered 
beginning  with  leg  one  and  proceed  in  a  clockwise  direction  through  leg  sk.  Each  leg  has 
three  degrees  of  fi’eedom  and  is  constructed  in  a  fashion  similar  to  manufacturing 
manipulators.  The  three  degrees  of  freedom  are  attained  with  three  articulations,  or  joints 
per  leg.  The  first  joint  rotates  about  a  vertical  axis  and  the  next  two  joints  rotate  about 
horizontal  axes.  The  joints  are  semi-direct  drive  with  DC  motor  actuators  and  combination 
harmonic  and  bevel  gears.  Each  of  the  18  joint  motors  requires  70  Volt  DC  power, 
provided  from  the  control  platform  via  the  tether  cable.  Each  motor  includes  an  encoder 
that  produces  100  pulses  per  revolution.  The  motor  control  system  is  shown  in  Figure 
A-2.  A  motor  driver  for  each  motor  counts  the  pulses  from  the  controller,  an  8086-based 
microcomputer,  and  from  the  motor  encoder  and  drives  the  motor  until  the  differential 
pulse  count  is  zero.  Using  this  method,  the  motors,  and  thereby,  the  joints,  are  rotated  to 
the  position  the  computer  commands.  [IWA88a] 


Figure  A-2.  Motor  Control  System  (After  [AKI89]). 
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Each  of  the  six  legs  has  an  end-efiector  consisting  of  a  25  centimeter  diameter,  three 
centimeter  thick,  disk  that  acts  like  a  foot.  This  foot  operates  from  a  passive  ball-and- 
socket  joint  at  the  end  of  the  leg.  This  passive  joint  constrains  the  motion  between  the 
long  axis  of  the  leg  and  the  surface  of  the  terrain  to  plus  or  minus  45  degrees. 

An  additional  three-degree-of-freedom  manipulator  is  attached  to  the  top  of  the  body 
and  provides  a  platform  for  a  TV  camera  and  ultrasonic  ranging  device.  Three  lilting  eyes 
are  mounted  equidistantly  around  the  upper  body  and  a  tether  interface  box  completes  the 
top  portion  of  the  body. 

AquaRobot  uses  four  sensor  types.  A  touch,  or  contact,  sensor  is  installed  near  the 
junction  of  each  leg  and  its  foot.  The  contact  sensors  have  a  two  millimeter  plunger,  but 
require  only  0.5  millimeters  of  travel  for  activation  [TAK93].  Two  inclination  sensors,  or 
inclinometers,  are  installed  in  the  body  to  determine  the  angle  of  inclination  with  respect  to 
the  terrain.  A  flux  gate  compass  is  also  installed  in  the  body  to  sense  direction.  Finally,  a 
pressure  sensor  is  included  in  the  base  of  the  body  to  accurately  measure  the  water  depth. 
[IWA88a] 

AquaRobot's  current  control  program  is  as  shoAvn  in  Figure  A-3.  The  program 
includes  walking  and  operating  algorithms  compiled  and  assembled  in  the  BASIC 
programming  language.  There  are  essentially  three  sections  of  this  program.  First,  there  is 
the  Walking  Algorithm  Program.  The  main  purpose  of  this  section  of  the  program  is  to 
receive  inputs  from  a  human  operator  and  convert  those  inputs  into  coordinate  values  for 
the  robot's  feet.  Then  the  Robot  Language  section  of  the  program  is  called.  The  Robot 
Language  section  includes  fundamental  command  functions  that  perform  operations  such 
as  changing  the  speed  of  motion  of  the  leg,  rotating  the  leg  joints  through  a  given  angle, 
and  determining  whether  a  touch  sensor  has  contacted  the  ground.  The  Robot  Operating 
System  Program  then  performs  the  necessary  operations  to  actually  move  the  robot. 


Calculation  of  joint  angles,  conversion  of  angles  to  motor  pulses,  and  providing  the  pulsed 
output  to  the  motors  is  included.  Finally,  the  Robot  Operating  System  Program  contains 
limited  graphical  display  and  simulation  functions.  [AKI89] 


WALKING  ALGORITHM  PROGRAM  ROBOT  OPERATING  SYSTEM  PROGRAM 


Figure  A-3.  AquaRobot  Computer  Control  Program  (After  [AKI89]). 

The  prototype  AquaRobot  was  actually  tested  in  the  field  environment  in  February 
1990.  The  test  area  chosen  was  the  temporary  storage  area  for  completed  caissons  at  the 
Izumi  working  area  in  the  Kamaishi  Port  of  the  Iwate  Prefecture.  There,  the  sea  bottom 
was  covered  with  a  rubble  mound  leveled  with  the  same  leveling  machine  used  on  the 
actual  barrier  site.  The  average  depth  was  24  meters.  The  navigational  test  is  performed  by 
having  AquaRobot  walk  fi'om  one  point  to  another  along  a  previously  measured  path. 
Rectangularly  shaped  one-centimeter  by  two-centimeter  steel  plates  are  used  to  mark  the 
waypoints  along  the  path.  Table  A-1  shows  the  results  of  navigation  and  walking  speed 
accuracy  during  the  field  test.  Table  A-1  shows  a  maximum  error  of  +21  centimeters.  This 
error  includes  the  average  navigational  error  of  the  transponder  system,  ±10  centimeters. 
Errors  may  derive  fi-om  aqueous  influences  (such  as  ocean  currents),  tether  influences,  or 
cumulative  foot  positioning  offsets.  From  this  table,  it  is  obvious  that  walking  speed 
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depends  upon  step  height  and  step  length.  The  maximum  walking  speed  attained  was  1.43 
meters  per  minute.  [IWA90] 


Table  A-1.  Navigation  and  Walking  Speed  Accuracy  (After  [iwa901). 


Step  Length 

(cm) 

Step  Height 

(cm) 

Measured 

Destinadon 

(cm) 

Error 

(cm) 

15 

35 

X 34821 

X 34816 

-5 

0.61 

Y-7251 

Y-7264 

-13 

20 

35 

X  35029 

X 35044 

+15 

0.67 

Y -8232 

Y-8218 

+14 

15 

35 

X 34821 

X 34822 

+1 

0.67 

Y-7251 

Y -7249 

+2 

20 

35 

X 35029 

X 35037 

+8 

0.76 

Y-8232 

Y-8231 

+1 

20 

25 

X 32810 

X 32811 

+1 

1.27 

Y -7371 

Y-7350 

+21 

20 

25 

X 33000 

X 32981 

-19 

1.43 

Y-7500 

Y-7504 

-4 

Figure  A-4  shows  the  results  of  AquaRobot  walking  a  planned  path.  The  planned 
path  included  start  and  goal  points  and  six  waypoints.  The  actual  walking  distance  was  9S 
meters.  In  this  case,  the  average  walking  speed  was  1.32  meters  per  minute.  [IWA90] 
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Figure  A-4.  Field  Test  of  Planned  and  Measured  Path  (After  (IWA90]). 
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APPENDIX  B: 


MATLAB  PROGRAM  LISTINGS 


This  appendix  contains  the  high  level  source  code  used  in  development  and  testing  of 
the  AquaRobot  gait  planning  algorithms.  The  source  code  is  in  Matlab.  a  ''C"-based 
development  language.  The  Matlab  code  is  not  intended  for  subsequent  use  in  either 
AquaRobot  or  the  AquaRobot  Simulator. 

Matlab  is  a  registered  trademark  of  The  Math  Works.  Incorporated.  It  does  not  produce 
stand-alone  executable  code,  but  is  instead  an  interpretive  program  designed  for  numerical 
problem  solving  and  is  especially  adept  at  matrix  manipulation.  The  source  code  in  this 
appendix  requires  Matlab  for  execution. 
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S  Title:  irciittjn 
S  Inputs:  (1^,4.5.6.7) 

SOutpuU:  [U^] 

SPuipose:  ThisMatUbfiinctioodetenninestheinleneGtionofany 
%  and  an  are  segnieoL 

% 

fimclioa  (intcpt,  dist,  flag]  >  aretDi(foot,  dine,  ceobod,  rad,  timneg,  limpoa,  flnum) 

% 

%  intcpt  =  (x,y)  oootdinates  td'intereept 

%  dist  =  distance  between  foot  and  intereepls 

%flag~  1  if  intercept  found;  flag  >*0  if  no  interoept  found 

%  foot  ~  (x,y)  foot  components 

S  direc  -  direction  of  interest 

Scenbod’*  (x,y)  coordinates  (^center  of  body 

S  rad  °  radius  oi  circle 

%  limneg/limpoa  ~  endponUs  of  are  sepnent 

%  flnum  =  foot  1  tfarou^  6 

% 

limit  ^  213  ;  H  maximum  possible  distance 
flag  =  0; 

direc  =  direc*pi/180; 

% 

%  Determine  if  the  foot  foils  on  the  are  segment 
footrad  =  «iit(foot(l,l)^  +  foot(l,2)^); 
if  (rad  fboUad) 
flag  -  1; 

intcpt  -  [foot(l,l),  foot(l,2)]; 
dist  =  limit; 
end 
% 

if  (flag  —  0) 

perpd  -  ((cenbod(l,2)  •  foot(l,2))*cas(direc))  •  ((oenbod(l,l)  •  fool(l,l))*sin(direc)); 
ifperpd<-rad 

len^  -  s<pt(rad^  -  perpd^); 

% 

%  find  (x,y)  at  intersection  of  perpd  and  length 
xperp  -  cenbod(l,l)  (pcrpd^in(direc)); 
yperp  =  cenbod(l,2)  -  (perpd*cos(direc)); 

34  lest  if  foot  is  inside  or  outside  radius 

radtest  =  s<p1((foot(l,2)  •  ceobod(U)r2  +  (foot(l.l)  •  ceiibod(l.l)r2); 
if  (radtesl  >  rad) 

%  find  (x,y)  at  intersection  of  ray  and  are 
xint  -  xperp  -  (lenglfa*cos(direc)); 
yint  =  yperp  -  (lengih*sin(direc)); 
intcpt  -  [xint,  yint]; 
else 

*/•  find  (x,y)  at  intersection  of  ray  and  are 
xint  -  xperp  +  (1ength*cos(direc)); 
yint  -  yperp  +  (Iength*sin(direc)); 
intcpt  -  (xint,  yint]; 
end 
U 

*/•  test  if  intersection  is  in  desired  direction 
indir  -  foot(l,l)*o<^direc)  +  foot(l,2)*sin(diiec); 
outdir  -  xint*cos(diiec)  +  yint*xin(direc); 
if  (indir  <=  outdir) 

dist  =  sqrt(  (yint  -  foot(l,2))^  +  (xint  -  foot(l,l))^  ); 

•/i 

*/o  test  if  intersection  is  within  are  segment 

arefoot  -  atan2(  yint  -  cenbod(l,2),  xint  -  cenbod(l,l)  ); 

narclim  =  atan2(  liiTB>eg(l,2)  •  cenbod(l,2),  limn^l,!)  -  cenbod(l,l)  ); 

parclim  -  atan2(  limpos(l,2)  -  cenbod(l,2X  linipos(l,l)  -  cenbod(l,l)  ); 

if  (flnum  =  1) 

if  (arefoot  >-  narclim)  &  (arefoot  <-  parclim) 
flag  -  1; 


«od 

ebe 

tf(«firac>0) 
if  (narclim  <  0) 
naidim  >  narclim  *  (2*pi); 
end 

if(paictim<0) 
parclim  ^  pardim  +  (2*pi); 
e^ 

if(arcfoot<0) 
a^oot  >  arcfoot  (2*pi); 
end 

if  (arcfoot  >>  naidim)  &  (arcfoot  <*=  parclim) 
flag-  1; 
end 

elae  %(direc<0) 
if(narclim>0) 
narclim  -  narclim  -  (2*pi); 
cod 

if(parclim>0) 
parclim  -  parclim  •  (2*pi); 
end 

if  (arcfoot  >  0) 
a^oot  arcfoot  •  (2*pi); 
end 

if  (arcfoot  >=  narclim)  &  (arcfoot  <-  parclim) 
flag  -  1; 
end 
end 
end 
end 
end 
end 

if  (flag—  1) 
intcpt-Q; 
dist-limh; 
flag-0; 
cod 

%  Title;  arcint2.m 
%  Inputs;  (1A3.4,5,6) 

%  Outputs;  [1.2^,4] 

%Puipose;  This  Midlab  function  determines  the  inteisection  of  a  directed 
%  my  and  an  arc. 

% 

function  [xint,yim,di$t41ag]  -  atcitit2(rx^/theta^,ycjna<i) 

% 

%  rad  -  radius  of  ciicle(arc  segment) 

%xc/yc-  coordinates  of  center  of  circle 
%  rx/iy  =  ray  eoo^)onents 
%  itheta  -  direction  of  interest 
%  xint/yint  -  arc  segment  intercepts 
%  dist  -  distance  between  rx/iy  xint/yint 
%flag-  1  ifinteicept  found;  flag -Oifno  intercept  found 
% 

patclini-30; 

Daiclim-30; 
rCjeta-(itheta*pi/l  80); 

% 

d-(yo-ry)*cos(itheta)-(xc4x)*sin(t1heta); 

ifd<rad 

lenglh-s(pl(nd'^-d'^); 

elseifdr=-rad 

lenglh-O; 

d-rad; 
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else 

xint-D; 

yint-Q; 

dist-Q; 

flagrO; 

end 

xpeiy“xc-Kd*sin(rtlieU)X 

yperp-yo-(d‘coe(rtheto)X 

nMkest-iqrt((ry-ycy^+(ix-xcy^); 

ifradtesOnd 

xiut-3q)eip-<l«°8th*oos(rthett)); 
yiut-yi»eip-(lenglh*iin(itheU)); 
ebeifra<kest<tad 
xiiit-xpenH<length*cas(ithe(s)); 
yint'^ypetp  *  (lengili*sin(ttliets)); 

%  elseif  iidtest=nd 
else 
fUg-0; 
end 
% 

Htest  ifintenectioa  is  in  desired  direction 

xpas>Tad*sin(parclim*pi/180>i-xc; 

ypos=rad*cas(parclim^i/l  80yi-yc, 

stcpos»aIan2(ypos-yinMpos-xiiit); 

xneg°Tard*sin(-narcto*pi/l  80)+xc; 

yneg-rad*oos(-fiarclim^l80>+yc; 

si<coep'atsii2(yint-yneg,xiiit>xneg}; 

testpos>:q>os*cos(srcpos>^ypas*sin(srcpas); 

testiieg-»>eg*oos(sraie^yneg*sin(arcneg); 

testislp~xiiit*oos(arcpas>)-yint*sin(srcpa8); 

te3tintii>xiiit*cos(srciie^+yint*siii(srcneg); 

if  (tesiintp<=testpos)  A  (testintii>'*testnes) 

dist-sqtt((yiot-ryy^-t<xint-txy^); 

else 

xint=Q; 

yint-Q; 

dist-Q; 

flapK); 

end 

^**.****.«M*»««»««**«..«***.**«*****..***.*.***.««*..*...****>*>***** 

%  Title;  b2wtntisjn 
%IiVuts:  (U.3.4,5.6.7,8^) 

%  Outputs:  [1] 

%Putpose:  This  function  ttansfanns  body  coordinates  to  world  coordinstes. 

^,  .**.*««.*.***•*.«***«»**•*«*•«•*•*«**..*.•••****••*•*••*••**»*•*•*•» 

% 

function  [world]  =  b2wtrans(plii,  theta,  psi,  xtrans,  ytrans,  ztrans,  td>,  yb.  d>) 

% 

%  phi  -  rotation  of  {B}  about  the  Xw-axis; 

%  theta  -  rotation  of  {B}  about  the  Yw-axis; 

*/4  psi  =  rotation  of  {B}  about  the  Zw-axis; 

%  xtrans  -  X-axis  translation  of  {B}  with  respect  to  { W}; 

%  ytrans  -  Y-axis  translation  of  {B}  with  respect  to  {W}; 

%  ztrans -Z-axis  translation  of  (B)  with  respect  to  {W}; 

%  xb  =  known  Xbody  coordinate; 

%  yb  -  known  Ybody  coordinate; 

%  zb  -  known  Zbody  coordinate; 

% 

%  construct  the  translation  vector,  the  body  vector,  and  the  rotation  matrix 
trans  =  [xtrans,  ytrans,  ztrans]'; 

body  =  (]d),ybAlT; 

rotate  =  [cos(psi)*cos(dieta)  cos(psi)*sin(theta)*siii(phi)-sio(psi)*cos(phi)  cos(psi)*sin(theta)*cos(phi>+sin(psi)*sin(phi); 
sin(p6i)*o^theta)  $in(psi)*si^theta)*sin(phi>Hos(psi)*oos(phi)  sin(psi)*sin(thela)*cos(phi>cos(psi)*sin^); 
-sin(tli^)  c^th^)*sin(idu)  cas(th^)*cos4^)]; 

•/. 


127 


%  buUd  the  bo<iy^warid  iMnogeoMiiS  matrix 
BVrr  ~  [route  tiaiia;0  0  0 1]; 

% 

%  find  the  reoihant  4X1  vector  of  cquivalcat  world  coordmatea 
TworW-=BWT»body; 

% 

S  strip  off  the  (x,y^)  coonliiiatea 

world  -  |Tw«rld(l.lX  Twaftd(2.1X  Twald(3,l)]; 

%  Title;  cydouQjn 
Hlqiutt:  radius  a,  stride  d 
%  Outputs; 

%Purpose;  This  Matlab calculates  a se^nentofa cycloid  and  eraphs  it. 

% 

^«.*«.*»*,.<****.*****.«.M.««..*...*«..«.««**...«.*..*.*»«***...*.». 

% 

clear 

dc 

clg 

ain>=  36.31;  HinputCStride;  '); 
dtheU  -  inpiit(TiewFoot:  J, 
a  «  ain/(2*piX 
t »  0;a/200;2^; 
x  =  a»(t-sin(t)X 
y  =  a‘(l-cos(t)); 
plot(x.y) 
grid 

titl«(X)ydoid  Foot  Motion') 
xlabelOStride  •  cm*) 
ylabelCHeigbt  •  an!) 
hold  on 

theU  =t(l,l)*pi/180; 

dt  =  2*dtheu*pi/180; 

xnew  =  a*((th^-)dt)  -  siii(theu+dt)); 

ynew  *  a*(l  •  cosCtheU-i^)); 

plot(xiiew,ynew,'g+-') 

holdo£f 

*/.TiUe;  el3.ffl 
%  Inputs; 

%  Outputs; 

%  Purpose:  This  Matlab  program  calculates  an  elliptical  foot  path. 

% 

%  T^VarZ  +  y''2/b^  =1,  a>b 
% 

hold  off 
cic 
clg 
clear 

pfic”input(Xnter  present  foot-X;  ■); 
pfy=0;  %inputCEi>ter  present  foot-Y;  y, 
dfic=input(Xnter  desired  foot-X;  y, , 
dfy=0;  */«inputCEnter  desired  foot-Y ;  y, 
a»(sqn((dfy-pfy)^2+(dfit-pfic)^))/2; 

%3et  aspect 
b=20; 

x=-a;a/20;a; 

ifpfx<dfx 

xreal=pfx;(2*a)/(lengtli(x>l);dfic; 

else 

xreal”p6c;-(2*ay(length(x)-l);dfic; 

end 

elpse=n; 

forp=l;length(x) 
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y-b»«irt(l-x(l4>r2/«'^); 

el|Mc-idpw.xi«U(l4>)  y]; 

end 

axii([0  elpuOcnglliCxXIHSO  elpie(U>20  ISO]) 

l>lol(dpM(:,l).eh»M(:^XW 

tiUe(nii|itic>l  Foot  Pith') 
xUbelCX  ooofdiiiitei,  (cm)) 
ylibel^  eoordiiiatci,  (cm)') 
hold  on 

plot(pficj)fy,'oc8) 

plot(dfic,dfy,'oc8) 

grid 

^  a****************************!******************************! 

%  Title:  ellipsejn 
Si  Inputs:  (1^) 

SiOu^Nits:  [1,2^] 

SiPuipose:  This  Matlahfiinctioa  calculates  in  ellipticil  foot  path  using 
Si  the  known  foot  position  and  the  desired  amount  of  foot 

Si  movement  in  the  Xdirectioa 

Si 

Si  x''2/a''2  +  y'W2  -  1;  a  >  b 
Si 

fonction  [elpse,  aspect,  inc] «  ellipse(pfic,  d&) 

Si 

pfy-0; 

dfy-0; 

a  -  (sqrt((dfy-pfyy2  +  (dfit-pfity'2)y2; 

Si  set  aspect 

b  >  20;  Si  20cm  is  nuudmum  height  (Z)  or  Link3  will  overlap  Link2 

c  ~  s(pt(abs(a''2  •  b^));  Si  absolute  because  b>a 

inc  =  -a:a/20:a;  Si  actiial  nunfoer  of  increments  will  probably  be  >300 

ifpfic<dfo 

xieal  -  p£c(2*aV(]ength(inc)-l):dfic; 
else 

xieal  ~  pfic-(2*a)/(lengib(inc>l):dfic; 
end 

elpse -Q; 

forp=  I:leoglh(iac) 
y  -  b*sqtt(l  -  ino(l.py'2/a'^); 
elpse  =  [elpse-,xreal(l,p)  y]; 
end 

aspect  =  c/s; 


^M******************************************************************* 

Si  Title:  invfciiLm 
•/ohqxits:  (1,23) 

SiOufouts:  [133.4] 

SiPurpose:  This  Matlab  fonction  provides  the  inverse  kinematics  of  the 
Si  AquaRobot  for  a  given  foot  location  (px,py,pz)  and  a  chosen 
%  teg  number  (1-6). 

./,*»*•»*****«*•*.**»•«•.«••••*«••*«««**•••***•**••*«**•*•«•*••*•••**.** 

Si 

fonction  [theta0d,tlietald,theta2d,tiieta3d]^vkin(le&px,pyj>z) 

Si 

Si  tbeiaO'd'=degiees;  without  'tf^radians 
Si 

a0=37.5; 

al=20; 

a2=50; 

a3=100; 

Si 

ifleg=l 
theta0=0*pi/180; 
elseifleg*“2 
theta0=-60*pi/l  80; 
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elscifleg->3 

theU0--120*pi/180; 

ebeifleg=««M 

thdaO--18O*pi/lS0; 

ebciflig  '•  3 
theU0>-240*pi/180; 
ebeifleg->>6 

theU0--300*pi/180: 

else 

1faet*(M>*pi/lS0; 

end 

thcUOd^thetaO*  1 SO^; 

% 

bl“iq^(px-»0*co«(theUO)y^+(py^d)*siii(thetjO)y^); 

cl=(p*^+py^-«0^-»>l'^y(2*t0»bl); 

tlieUl(t-ataii2(s<|rt(l<l^Xcl)*180/pi; 

% 

b2=bl-el; 

b3“iqft(b2'^+ia^X 
c2=(«2'^+M'^-»3^y(2*a2»b3); 
theU2iB-ataii2<-sqrl(  l-c2^),c2)-aUii2(pz,b2); 
theu2*ataii2(s^l-c2^Xc2yitaii2(pz.b2X 
if(theU2<-106.6‘pi/l*0)&(theta2>=-73.4»pi/180) 
theta2<Hlieta2*180^; 
else 

thete2^^heta2iii; 

theU2d°°theU2*180/pt; 

end; 

% 

c3-(b3^-)i2'^-i3'^y(2*s2»s3); 
tbeU3m*‘sun2(-eqrt(l-c3^),c3); 
tbeta3~ataD2<s^l-c3'^Xc3); 
%if(theU3<«156.4V180)&(theU3>=-23.6»pi/180) 
%  tliets3tiKta3m; 

%  tbeti3<Hhets3*180/pi; 

%ehe 

%  theU3d-theta3*180/pi; 

^iead; 

if  thets3>=0*180/pi 
theU3d-tbeU3*480/pi; 
else 

theU3^'tlieU3ni; 

theU3d=aieU3*180/pi; 

end; 


%  Title:  jcoordm 
%  Inputs: 

^Ou^Mts: 

%  Purpose:  This  Matlab  program  cbedcs  the  kinemsticsoflheAqusRoboL 
%  Joints  Zero,  One,  Two,  and  Three  are  used. 

% 

Idelc 
diary  c 

angleO>‘itq)Ut(Tnter  Leg  angle  (0,60,120,180,240,300):  y, 
anglel=itg)UtCEnter  Hip  angle  :  y, 

angte2'‘input(Tnter  Kneel  angle  :  '); 

angle3°°iiiput(Ti)iter  Knee2  angle  :  y, 

diary  off 
% 

angle0=angle0*pi/l  80; 
anglel=anglel*pi/180; 
angle2’‘angle2*pi/180; 
angle3>'angle3  *pi/l  80; 

•/. 

aO-37.5; 
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*1-20; 

•2=30; 

a3~100; 

% 

TBO-{c<n(MigteO)  -ciii(aiigleO)  0  O', 
siii(iii^0)  oai(aiigieO)  0  0; 
0010; 

0001]; 

% 

T01>[cat(aiigt6l)  -ctii(aiiglel)  0  «0; 
siii(aii^l)  coi(aiiglel)  0  0; 
0010; 

0001]; 

% 

diaiy  c 

H1P-TB0*T01 
pause 
diaiy  off 
% 

T12>[coa(aiig|e2)  -sin(aiigle2)  0  al; 

0010; 

-ain(aiigle2)  -caa(aii^e2)  0  0; 

0001]; 

% 

diaiyc 

KNEE1-TB0*T01»T12 
pause 
diaiy  off 
% 

T23-[oos(aog|e3)  -siiiCaiigteS)  0  a2; 
siii(an^e3)  oos(sngle3)  0  0; 

0  010; 

0001]; 

•A 

diaiyc 

KNEE2-TBO*T01»T12*T23 

pause 

diary  off 

•A 

T34=[10  0a3;  - 
010  0; 

0  010; 

0001]; 

•A 

diaiyc 

FOOT=TBO*T01*T12»T23*T34 
diaiy  off 


•^  .**«*«*«**.«*«***«**********«*«*»*«.«**«*«**•***••*••••••«••«•* 

%  Title:  jkin0123jn 
%  Inputs: 

%  Outputs: 

%Puipose:  This  Matlab  program  checks  the  kinematics  of  the  AquaRobot 
%  Joints  Zero,  One,  Two,  and  Three  are  used. 

s^*************************************************************** 

% 

!delc 

diaiyc 

angieOHnputfEnter  Leg  angle  (0,60,120,180,240,300):  y, 
anglel^HnputCEnter  Hip  angle  :  '); 

angle2~ii9Ut('Enter  Kneel  angle  :  J, 

angle3=ini>ut('Enter  Knee2  angle  :  y, 

diaiy  off 
% 

angle0~angle0*pi/180; 

anglel’'angtel*pi/180; 

angle2°angle24>i/180; 
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angjc3-aogle3*pi/180; 

% 

•0-37.5; 

•1-20; 

•2-50; 

V, 

TBO-(otM(«igtcO)  •«iii(«igte0)  0  0; 
smC^iigtcO)  co((^iigM)  0  0; 

0010; 

0001]; 

% 

T01-(ca«(^iiglel)  -onC^nglcl)  0  •O; 
sin(^ii^l)  co^Migiel)  0  0; 

0010; 

00  01]; 

H 

dUry  c 

HIP-TB0*T01 

puise 

£aryoS 

% 

T12-(ooi(^iigle2)  -cill(•llgie2)  0  al; 

0010; 

-«iii(aii^e2)  •ca*(aiigle2)  0  0; 

0001]; 

•A 

diaiyc 

KNEE1-TBO*T01*T12 
pause 
dtaiy  off 
% 

T23-[cas(angle3)  -siii(aiigle3)  0  ^2; 
sin(aiig|c3)  oo^angleS)  0  0; 

0010; 

000  1]; 

% 

diaiyc 

KNEE2-TB0*T01*T12»T23 

pause 

diaiy  off 

%  Title:  jkiiil23.in 
S  Inputs: 

SOu^iuis: 

^Purpose:  This  Matlab  program  checks  the  kinematics  of  the  AquaRoboL 
%  Joints  One,  Two,  and  Three  are  used. 

^.a.*****.*....************************************************* 

% 

ideic 

diaiyc 

anglel-inputCEnter  Hip  angle  :  Of 

angle2-iiqiut('Eiiter  Kneel  angle  :  *}; 

angle3-input('Eiiter  Knee2  angle  :  *); 

diaiy  off 

•/. 

anglel-anglel*pi/l  80; 
ang|e2=angle2*pi/l  80; 
angles -angle3*pi/180; 

% 

•0-37.5; 

al-20; 

a2=50; 

•3=100; 

% 

T01=[cos(anglel)  -sin(anglel)  0  aO; 
sin(anglel)  co^anglel)  0  0; 
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T12«[ciM(aiigle2)  -f>n(aiigle2)  0  al; 

0010; 

■aiii(aiigte2)  -oat(iagle2)  0  0; 

0001]; 

duty  C 

KNEE1-T01T12 

^txyoS 

T23-[oo((angle3)  -aiii(aiigte3)  0  a2; 
siii(aiig|c3)  oa<aiigie3)  0  0; 

0010; 

0001]; 

diaiy  c 

KNEE2-T01»T12*T23 

pause 

diary  off 

%  Title:  kiii2.m 
%  Inputs:  (UA4) 

%  Outputs:  [1^.4] 

HPuipose:  This  Matlabfimctiaa  provides  the  kinematics  of  the  AquaKobot 
%  fora  given  set  of  joint  angles.  Uses  new  LINK  lengths. 

^*««*..*«**«««*.M«*.«**«..«*.«**»**«»*«***«*********.**»******* 

% 

fonction  [HipJCneel  JCnee2,Foot] «  kin2(theta0,thelal,theta2,theta3) 

% 

angle0"theta0*pi/l  80; 
anglel  •■thetal  *pi/l  80; 
angle2>theta2*pi/180; 
angle3>theta3*pi/180; 

% 

a0=37.5; 

al-21.5; 

a2-52; 

a3>101; 

% 

TB0=[oos(angle0)  -sin(angle0)  0  0; 
sin(angle0)  co^angleO)  0  0; 

0010; 

0001]; 

% 

T01>[co$(anglel)  -sin(anglel)  0  aO; 
sin(anglel)  co^anglel)  0  0; 

0010; 

0001]; 

% 

H=TB0*T01; 

% 

T12=[cos(angle2)  -5in(angle2)  0  al; 

0  010; 

-sin(an^e2)  -cos(angle2)  0  0; 

0001]; 

% 

K1=TB0*T01*T12; 

% 

T23=[caf(angle3)  -sin(angle3)  0  a2; 
sin(angle3)  cos(angle3)  0  0; 


K2-TB0»r01*T12»T23; 

% 

T34-{100»3; 

0100; 

0010; 

0001]; 

% 

FT-TBO»T01*T12*T23*T34; 

S 

Hip-(H(1,4)H(2,4)H(3.4)1; 

Kiieel-[K1(1,4)  Kl(2.4)  Kl(3.4)]; 

Knec2-[K2(1.4)  K2(2.4)  K2<3,4)]; 

Foot-[FT(l,4)  FT(2.4)  PT(3,4)]; 

^  ••*••••••••••*•••••»•*••*•*««••••*•••••••••••••••••••••••••*••••••••• 

Slltle:  nMUcdm 
Hbpiitt:  (U3,4.3,6.7.W) 

%Oul])iib:  [1] 

HPufiraae:  ThbMatUbiuDclioafinditlieiDudmuinsIndepoMtUefar 
%  AquaRobot,  given  an  ariMtruydirectiaa  as  an  input 

U 

function  [niaxdiat]~inaxd(fbotl,foot2,foat3^oot4/ootS/oot£,<iirec,oeid>od,lri) 

% 

S  maxdist  maximum  foot  movement  in  the  direction  requeited 
%  fiiotl  jootti  -  (x,y)  coordinatei  of  fiMt 
H  dime  -  diiectian  of  ntterert 
S  oenbod  •  (x,y)  coordinates  of  the  center  of  the  body 
34  tri  •  tripod  oianbcr  0  (lege  14,3)  or  number  2  (legi  2,4,6) 

% 

%  •••••  MUST  CONVERT  (X,Y,Z)WORlD  COORDINATES  TO  BODY  COORDINATES  HERE 
S 

H  initializationa  here 
% 

inrad  37.3;  %  inner  radius 

outrad  >  178.2107;  %  outer  radius 
34 

seglpin  ~  [36.8686, 6.8324];  34  inside  endpoint  of  positive  segment  #1 
seglpout  •  [160.2049, 78.0606];  34  outside  en^int  of  positive  segment  #1 
seglnin  >  P6.8686,  •6.8324];  %  inside  endpoint  of  negative  segment  #1 
seglnout  -  [160.2049,  -78.0^];  34  outside  endpoint  of  negative  segment  #1 
34 

segSpin  ^  [12.3, 334333];  34  inside  endpoint  ofpositive  segment  #2 
seg2pout  ■>  [12.3, 177.7718];  %  outside  endpoint  of  positive  se^nent  #2 
seglnin  *•  [24.3686, 28.3030];  34  inside  endpoint  of  negative  segment  #2 
se^nout « [147.7049, 99.71 12];  34  outside  endpoint  of  negative  segment  #2 
34 

seg3pin  >  [-24.3686, 28.3030];  %  inside  endpoint  of  positive  segment  #3 
seg3pout  >  [-147.7049, 99.71 12];  34  outside  endpoint  of  positive  segment  #3 
seg3nin  -  [-12.3, 334333];  34  inside  endpoint  of  negative  segment  fO 
seg3nout  -  [-12.3, 177.7718];  34  outside  endpoint  of  negative  segment  #3 
34 

seg4pin  >  [-36.8686,  -6.8324];  34  inside  endpoint  of  positive  segment  #4 
seg4pout  •  [-160.2049,  -78.0606];  34  outside  endpoint  of  positive  segment  #4 
seg4nin  >  [-36.8686, 6.8324];  34  inside  end-point  of  negative  segment  #4 
segdnout » [-160.2049, 78.0606];  34  outside  endpoint  of  negative  segment  #4 
34 

seg3pin  ■■[-12.3,-333333];  %  inside  endpoint  (^positive  segment  #3 
seg3pout  -  [-12.3,  -177.7718];  %  outside  endpoint  of  positive  segment  #3 
seg3nin  •  [-244686,  -28.3030];  34  inside  endpoint  of  negative  segment  #3 
seg3nout  •  [-147.7049,  -99.71 12];  34  outside  endpoint  of  negative  segment  #3 
34 

seg6pin  =  [24.3686,  -28.3030];  34  inside  endpoim  of  positive  segroem  #6 
seg6paut  =  [147.7049,  -99.71 12];  34  outside  endpoint  of  positive  segment  #6 
seg6nin  =  [12.3,  -33.3333];  34  inside  endpoint  of  negative  segment  fi6 
seg6nout  ~  [12.3,  -177.7718];  34  outside  ent^intofnegative  segment  #6 
% 
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while  (tri  “  0) 

%tertfoatl  in  kg  1  woriupeoe 

ft-1; 

[inlciit,  dull.  flag]-arant(footl,  direc,  ceobod,  outnd,  segloout,  xeglpout,  ft); 
if(fUg“  1) 

(hsU^distl; 

end 

(iolcpt,  distl,  flag]-aiGinl(ft)a(l.  doec,  cenbod,  imd.  seglnin.  seglpin.  ft); 
if(flig“  1) 

(hsU  -  dittl; 
end 

[inlGiit,  dikl,  jBag]-wgml(fi)otl.  dine,  s^lpni.  seglpout); 
if(fl*g—  1) 
ihfU-diitl; 
end 

[intept,  dull.  flag]*tegiiii(footl,  dine,  seglnin,  seglnout); 

if  (flag— 1) 

<hiU~distl; 

end 

% 

%  test  foot3  in  kg  3  workspace 
ft -3; 

(mtept,  distS,  fk^arctnt(ft>oC3,  dine,  cenbod,  outnd,  seg3nout,  segSpout,  ft); 
if  (flag  —  1) 

ead 

[inept,  diets,  flag]-arcin(ft>at3,  diree,  eenbod,  inrad,  segSnin,  segSpin,  ft); 

if  (flag  a»  1) 
a  dlSt3*, 
end 

[inept,  diets,  flag]°>eegin(foot3,  diree,  segSpin,  segSpoutX 
if  (flag"  1) 
a  dist3; 
end 

(inept,  diets.  flagJ-eeginffoatS.  diree,  segSnin.  seg3nout); 
if  (flag—  1) 
dietb^dietS; 
end 
% 

S  test  foots  in  leg  S  wofkspace 

ft*  J; 

[inept,  diets,  flag]*arcint(footS,  dine,  cenbod,  outrad,  segSnout,  segSpout,  ft); 
if(flag—l) 

—  dist5; 
end 

[inept,  diets,  flag]=arctn(foatS,  dine,  cenbod,  inrad,  segSnin,  segSpin,  ft); 
if(flag—  1) 
ihstc  =  diets; 
end 

[intqit,  diets,  flag]~segin(foatS,  diree,  eegSpin.  segSpout); 
if  (11^—  1) 
distc  =  distS; 
end 

[intept,  diets,  flagl^'seginffoatS,  diree,  segSnin,  segSnout); 
if  (flag— 1) 
dieted  diets; 
end 
% 

inaxdist  -  min(  0iin(dieta,  distbX  dietc); 

% 

break; 

end  %  this  ends  tripod  0  testing 
% 

while  (tri  —  2) 

%  test  fool2  in  leg  2  workspace 
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ft -2; 

[ialG|il.  flag]>ananl(fi>at2.  direc.  ocobod,  outiad,  <eg2iiout,  ttglpaut,  ft); 
1) 

dlUrta  *  dtlt2*, 

end 

[iniqpt,  dtit2,  flag]*'a(cinl(fi>ot2,  direc.  cenbod,  imd.  seglaia,  le^pitt,  ftX 
1) 

distft  *  diit2» 
cad 

(mtcpt,  diit2.  flag]-seginl(fi>ot2,  direc,  ceg^pin.  ceg^pouty, 
if(fUg"  1) 

dli«ta  ai  dtft2i 

cad 

[iatqpt,  diit2,  flag]-iegiat(foat2,  direc,  le^mn,  tetflaout); 
if(flag“-  1) 

(Usts~diit2; 

end 

% 

%  test  fi>ot4  in  leg  4  watkijnce 
ft -4. 

[intqit,  dist4.  fUg]>erciitt(foot4,  direc,  cenbod,  oiitnd,  cegdaout,  teg4pout,  ft); 
if  (flag  =1) 
distb~diit4; 
end 

[intcpi,  dist4,  flag)>areint(foot4,  direc,  cenbod,  innd,  seg4oin,  seg4pin.  ft); 
if(fUg=.l) 

ifisib  =  diat4; 
end 

[intcpt,  dist4,  flag|-segint(fi>ot4,  direc,  >eg4pin,  seg4poiit); 
if(flag="  1) 

(Uftb  ■■  didV, 
end 

(inl^  diA4,  iUg]>*iegint(foot4.  direc,  aeg4nin.  seg4nout); 
if(flag=“  1) 

^itb»dtet4; 

end 

% 

S  test  fooCd  in  leg  6  woifcspace 
ft“<i; 

[intcpt,  dist6.  ilag]>arcint(fbot6,  direc,  cadxxl,  outred,  seg^iout,  seg6pout,  ft); 
if  (flag  —  1) 
a  dlSt6^ 
end 

[intcjit,  dist6,  fiag]»arcint(foot6,  direc,  cenbod,  innd,  s^6nin,  seg6pin,  ft); 
if  (flag— 1) 

end 

[iniqpt,  dist6,  flag]>segiiit(foot6,  direc,  segfipin,  segfipout); 
if  (flag— 1) 

«!■<*<*  V  dist6’, 
end 

[intcpt,  dist6,  flag]>segint(foot6,  direc,  seg6nin,  segdnout); 
if  (flag  —  1) 
distc>dist6; 
end 
% 

maxdist  >  inin(  niin(dista,  distb),  distc); 

% 

break; 

end  %  this  ends  tripod  1  testing 


./.***••**•*•«***««•****••*•****•*««•••••**«•**•**••*•••••**•*•. 

%  Title:  inaxd2S.m 
•/iligwts;  (1,23,4.5,6.7,8,9) 

%  Outputs:  [1] 

%Puipose:  TteMatlab  function  finds  the  maximum  stride  passible  for 
%  AquaRobot,  given  an  aibitrary  direction  as  an  input 
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TliisfiiiictiooafortfaeS}  on  footpad. 


% 

% 

% 

function  [maxdut]^niaxd25<footljroot2.foot3,fi>ot4/oot3/oot6,difec,ca>bod,th) 

% 

%  maxdist  =  maxiinum  foot  movemeot  in  the  directioa  requested 
%  fixitl  ifootfi  -  (x,y)  coordinates  feet 
%  diicc  =  diiectioa  of  interest 
%  cenbod  ^  (x,y)  coordinates  of  the  center  rtf' the  body 
%tri- tripod  number  0(1^9  13.S)  or  number  2  (legs  2,4,6) 

% 

%  •••••  MUST  CONVERT  (X,Y,Z)WORLD  COORDINATES  TO  BODY  COORDINATES  HERE 
% 

inhialiTations  here 

% 

inrad  37.S;  %  iimer  radius 

outrad=  178.2107;  %  outer  radius 
% 

seglpin  ~  P6.8d86, -d.8524];  H  inside  OK^xnnt  of  positive  segment#] 
seglpout  -  [160.2049,  -78.0606];  %  outside  endpoint  of  positive  seffoeal  #1 
seglnin  =  [36.8686, 6.8324];  %  inside  endpoint  of  negative  ttffoeA  #1 
seglnout  >  [160.2049,  78.0606];  %  outside  endpoint  of  negative  se^nent  #1 
•/. 

seg6pin  =  [12.3, 33.3333];  inside  endpoiid  of  positive  sepnent  #2 
3eg6pout  ==  [12.3, 177.7718];  %  outside  endpoint  of  positive  segmeid  #2 
seg6nin  -  [24.3686, 28.3030];  34  inside  endpoint  of  negative  se^nent  #2 
seg6nout  =  [147.7049, 99.71 12];  %  outside  endpoint  of  negative  se^nent  #2 
% 

seg3pin  -  [-24.3686, 28.3030];  34  inside  endpoint  of  positive  segment  #3 
seg3pout  =  [-147.7049, 99.71 12];  34  outside  endpoint  of  positive  se^noat  #3 
seg3nin  >  [-12.3, 33.3333];  34  inside  endpoint  of  negative  segment  #3 
seg3nout  -  [-12.3, 177.7718];  34  outside  em^wint  of  negative  segment  #3 
34 

seg4pin  =  [-36.8686, 6.8324];  34  inside  endpoint  of  positive  segment  #4 
seg4pout  >  [-160.2049, 78.0606];  34  outside  endpoint  of  positive  segment  #4 
seg4mn  =  [-36.8686,  -6.8324];  %  inside  endpoint  of  negative  segment  #4 
seg4nout  >  [-160.2049,  -78.0^];  %  outside  endpoint  of  negative  segment  #4 
•/« 

seg3pin>  [-12.3,-33.3333];  34  inside  endpoint  ofpositive  segment  #3 
seg3pout  "  [-12.3,  -177.7718];  34  outside  endpoint  of  positive  segment  #3 
seg3nin  -  [-24.3686,  -28.3030];  34  inside  en^int  of  negative  segment  #3 
seg3nout  =  [-147.7049,  -99.7112];  34  outside  endpoint  of  negative  segmeid  #5 
% 

seg2pin  =  [24.3686,  -28.3030];  34  inside  endpoint  of  positive  segment  #6 
seg2pout  [147.7049,  -99.71 12];  34  outside  endpoint  of  positive  segment  #6 
seg2nin  =  [12.3,  -33.3333];  34  ituide  endpoint  of  negative  sepnent  #6 
seg2nout  =  [12.3,-177.7718];  34  outside  endpoint  of  negative  segment  #6 

3. 

flag  “  0; 
while  (tri  “=  0) 

34tesl  footl  in  leg  1  woilcspace 
ft  -  1. 

(mlcpt.  distl,  ilag]-arcint(footl,  direc,  cenbod,  outrad,  seglnout,  seglpout,  ft); 
if  (flag  ==  1) 
disu  =  distl; 
end 

(intept.  dist  I ,  flag]=arcint(footl,  direc,  cenbod,  inrad,  seglnin,  seglpin,  ft); 
if(nag==l) 
dista  ==  distl; 
end 

[intept,  distl,  flag]=-segiiit(footl,  direc,  seglpin,  seglpout); 
if  (flag  ==  1) 
dista  =  distl; 
end 

[intept,  distl,  flag]=segint(footl,  direc,  seglnin,  seglnout); 
if  (flag  =1) 

(hsta  ■=  distl; 
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end 

% 

H  tcA  foots  in  leg  3  woilcspace 
ft«3; 

[intciit,  diets,  flag]~wcint(foat3,  dtiec,  cenbod,  outnd,  seg3nout,  legSpout,  ft); 
if  (flag  =1) 
distb  “  dist3; 
end 

[intqit,  diets,  fUg]°afGint(foot3,  direc,  cenbod,  inrad,  eegSnin,  segSpin,  fl); 
if(fleg“  1) 

riirth  3B  distS^ 

end 

[intcpt,  diets,  fIeg]>segint(foot3,  direc,  segSpin,  eegSpout); 
if  (flag  =1) 
dtsCb  ^  dist3^ 
end 

[intcpt,  diets,  flag]=segiid(foot3,  direc,  segSnin,  segSnout); 
if(flag=  1) 
distb  =  diets; 
end 

%  teet  foots  in  leg  S  woricepace 
11  =  5; 

[intcpt,  diets,  flag]=arcint(footS,  direc,  cenbod,  outnd,  eegSnout,  eegSpout,  ft); 
tf(flag=  1) 

K  diets; 

end 

[intcpt,  diets,  ilag]°=atcint(footS,  direc,  cenbod,  inrad,  eegSnin,  segSpin,  ft); 
if(flag*=  1) 
distc~  diets; 
end 

[intcpt,  diets,  flag}=5egint(footS,  direc,  segSpin,  segSpout); 
if  (flag  “=  1) 
dietc^  diets; 
end 

[intcpt,  diets,  flag]=5egint(ft>otS,  direc,  segSnin,  segSnout); 
if  (flag  =1) 
distc>=dieC; 
end 
% 

niaxdist  =  niin(  min(dista,  dietb),  dietc); 

% 

break; 

end  %  this  ends  tripod  0  testing 
% 

- -  .I  -  - .  - - -  ■ 

while  (tii  =  2) 

%  test  foot2  in  leg  2  workspace 
ft  =  2; 

[intcpt,  dist2,  flag]=arcint(fbot2,  direc,  cenbod,  outrad,  segioout,  seg2pout,  ft); 
if  (flag  —  1) 
dista  dist2; 
end 

[intcpt,  dist2,  flag]=arcint(foot2,  direc,  cenbod,  inrad,  segSnin,  seg2pin,  ft); 
if(flag==l) 
dista  =  dist2; 
end 

[intqit,  dist2,  flag]^=segint(foot2.  direc,  segSpin,  eegSpout); 
if(flag=l) 
dista  =  dist2; 
end 

[intcpt,  diets,  flag]=segint(foot2,  direc,  segSnin,  segSnout); 
if(flag=l) 

(hsta  =  dist2; 
end 
% 

%  test  footd  in  leg  4  wwkepace 
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11*4; 

(iniqit,  dixt4,  lUg]*arcml(fi>ot4,  direc,  ccabod.  outnd,  K84iiout.  seg4poiit,  ftX 

didb  *  diit4; 
eod 

[inicpt,  dist4,  flag]=arciiit(fiiot4.  dtrw,  ccnbod.  imd,  >eg4iiiii,  seg4ptii,  ft); 
if  (flag  “  1) 
iflstb  *  <liat4; 
end 

(intcpt,  dist4,  flag]*segiiit(foot4,  direc,  seg4pin,  segdpout); 
if(flag==l) 

(Ustb*dist4; 

end 

[intqpt,  dist4,  flag]*segint(foot4,  direc,  seg4ain,  wgdnout); 
if  (flag  =  1) 
di$tb*dist4; 
end 
% 

44  test  footd  in  leg  6  workspace 
ft*6; 

[intqit,  distd,  flag]*arcinl(foat6,  direc,  cenbod,  outrad,  segdiKxit,  segdpout,  ft); 
if(flag>*l) 
s  dist6; 
end 

[iitfcpt,  distd,  flag]*arcint(foot6,  direc,  cenbod,  inrad,  segdnin,  segdpin,  ft); 
if  (flag  =  1) 
at  diist6‘, 
end 

[intcpt,  distd,  flag]-segint(foot6,  direc,  segdpin,  segdpout); 
if  (flag  =1) 
distc*dist6; 
end 

[iincpt,  distd,  flag]*segint(foot6,  direc,  segdnin,  segdnout); 
if(flag=  1) 
distc  *  distd; 
end 
% 

maxdist  *  min(  inin(dista,  distbX  distc); 

% 

break; 

end  %this  ends  tripod  1  testing 


%  Title:  maxd4S.m 
%  Inputs;  (1,2,3,4,5,6,7,8,9) 

%  Outputs:  [1] 

%Putpose;  This  MatUbfimetion  finds  the  maximum  stride  possible  for 
%  AquaRobot,  given  an  arbitrary  direction  as  an  input 
%  This  function  is  for  the  43  cm  fooqnd, 

0,^  «***«•**«*»«•••*****»******•****•**•«••«***•*•*•****•.••*•**•••*** 

% 

function  [maxdist]=maxd4S(footl/oot2/oot3/oot4/oot3/oot6,direc,cenbod,tri) 

% 

44  maxdist  =  maximum  foot  movement  in  the  direction  requested 
%  footl  :foot6  =  (x,y)  coordinates  of  feet 
%  direc  =  direction  of  interest 
%  cenbod  *  (x,y)  coordinates  of  the  center  of  the  body 
%  tri  *  tripod  number  0  (legs  1,3,3)  or  number  2  (legs  2,4,6) 

% 

%  •••••  MUST  CONVERT  (X,Y,Z)WORLD  COORDINATES  TO  BODY  COORDINATES  HERE 
% 

%  initializations  here 
% 

inrad  *  43.0;  %  inner  radius 

outrad  =  149.27;  %  outer  radius 
% 

seglpin  *  [43.0, 0.0];  %  inside  endpoint  of  positive  segment  #1 


139 


wglpout  •  (139.0446, 54.2967];  S  outside  eodpoiot  of  poedivc  icgmeot  #1 
■eglnin  »  [45.0, 0.0];  54  ioeide  endpoint  of  negative  ae^neot  #1 
leglnout  -  (139.0446,  -54.2967];  H  outside  cadpoud  of  n^ative  segment  #1 
% 

seg^pin- [22.5, 38.9711];  54  neidecndtioint  of  positive  aqm*B4^ 
seg2pout  -  [22.5, 147.5645];  54  outside  endpoint  of  positive  a^aent  #2 
seg2nm- [22.5,38.9711];  54  inside  endpoint  ofne^tivesc0tieat  #2 
seg^nout  >  [1 16.5446, 93.2678];  %  outdde  endpoint  of  negative  segment  #2 
% 

seg3pin- [-22.5, 38.9711];  54  ins^  endpoint  ofpositiveseptient  #3 
segSpout  >  [-1 16.5446, 93.2678];  54  outside  endpoint  of  positive  segment  #3 
aeg3nin- [-22.5, 38.9711];  54  inside  endpoint  of  negative  segment  #3 
segjnout  -  [-22.5, 147.5645];  S  outside  endpoint  of  negative  seffnent  #3 
54 

seg4pin  =  [-45.0. 0.0];  54  inside  endpoint  (^positive  segnent  #4 
seg4pout  •  [-139.0446,  -54.2967];  S  outside  endpoint  of  positive  segnent  #4 
seg4nin  ~  [-45.0, 0.0];  %  inside  endpoint  of  negative  sequent  #4 
seg4nout  -  [-139.0446, 54.2967];  54  outside  en^int  of  negative  sepnent  #4 
54 

seg5pin  =  [-22.5,  -38.9711];  54  inside  endpoint  of  positive  segmant  #5 
ieg5pout  •  [-22.5,  -147.5645];  54  outside  endpoint  positive  segment  #5 
seg5nin  >  [-22.5,  -38.971 1];  54  inside  endpoint  of  negative  segment  #5 
seg5nout  -  [-1 16.5446,  -93.2678];  54  outside  endpoint  of  negative  segment  #5 
54 

3eg6pin  >  [22.5,  -38.971 1];  54  inside  endpoiid  of  positive  se^nem  #6 
seg6pout=  [116.5446, -93.2678];  54  outside  en^intofposhive  segment  #6 
segdnin  =  [22.5,  -38.971 1];  54  inside  en%>oint  of  negative  segment  #6 
segdnout  -  [22.5,  -147.5645];  54  outside  endpoint  of  negative  sepnent  H6 
54 

flag'O; 
vdule  (In  “  0) 

54testfootl  in  leg  1  wotfcspace 

ft=l; 

[intept,  distl,  flag|=arcint(footl,  diiec,  cenbod,  outnd,  seglnout,  seglpout,  ft); 
if(flag=l) 
dista^  distl; 
end 

[intept.  distl,  lUg]«*egint(fi>otl,  diiec,  seglpin,  seglpout); 
if(flag=l) 
dista  =  distl;  - 
end 

(intept,  distl,  flag]=segint(footl,  diiec,  seglnin,  seglnout); 
if(flag=  1) 
dists  =  distl; 
end 
% 

54  test  foots  in  leg3  woiicqnoe 
ft  =  3; 

(intept,  dist3,  flag]=arcint(foot3,  diiec,  cenbod,  outrad,  seg3nout,  seg3pout,  ft); 
if  (flag  “  1) 
distb  dist3; 
end 

(intept.  dist3,  flag]=segiiit(foot3,  diiec,  seg3pin,  seg3pout); 
if  (flag  =  1) 
distb  =  dist3; 
end 

(intept,  dist3,  flag]=segint(foQt3,  diiec,  seg3nin,  seg3nout); 
if  (flag  ==  1) 
distb  =  dist3; 
end 
% 

%  test  foot5  in  leg  5  woikspace 
ft  =  5; 

[intept,  dist5,  flag]=aTcint(foot5,  diiec,  cenbod,  outrad,  seg5iK>ut,  seg5pout,  ft); 
if  (flag  =  1) 
distc  =  dist5; 
end 
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(iKopt,  flag]>(eginl(faotS,  diicc,  segSpin,  legSpout); 
if(fliig“  1) 
m  dij|5‘ 

Old 

difl5.  flag]-ieginl(fi>atS,  direc,  segSnin,  segSnout); 

1) 

A*!/*  s  dist5; 
end 

Vt 

maxdist  -  inin(  min(disu,  distbX  dttfe); 

% 

bfcak; 

end  %  this  ends  tripod  0  toting 
% 

while  (tri  ■“  2) 

%  test  foot2  in  leg  2  woricspscc 
ft -2; 

[inlcpl.  dist2,  fbg]=arciin<foot2,  direc,  cehbod,  outrsd,  tcg2nout,  seg2pout,  ft); 
if(flsg“  1) 

SB  dist2^ 
end 

(intept.  dist2,  flsg]-segtnt(foot2,  direc,  seg2pin,  seg2pout); 
if  (flag  “  1) 

end 

[intept,  dist2,  flag]>segint(ft>ot2,  direc,  seg2nin,  seg2noiit); 
if(flsg“  1) 
distJi  *  dist2; 
end 
% 

%  test  fi>ot4  in  leg  4  woricspscc 
ft  =  4; 

[intept,  dist4,  ilsg]=*srcint(foot4,  direc,  cenbod,  outrsd,  seg4oout,  seg4pout,  ft); 
if  (flag  =1) 
distb>dist4; 
end 

[intept,  dist4,  flag]>segint(foot4,  direc,  seg4pin,  seg4pout); 
if  (flag  —  1) 
distb  =  dist4;  - 
end 

[intept,  dist4,  fUg]"segint(foot4,  direc,  seg4ain,  $eg4nout>, 
if(flag==  1) 
distb  =  di$t4; 
end 
•/. 

%  test  foot6  in  leg  6  workspace 
ft  =  6; 

[intept,  dist6,  flag]=arcint(foot6,  direc,  cenbod,  outrad,  seg6iiout,  seg6pout,  ft); 
if(flag=l) 
distc  =  dist6; 
end 

[intept,  dist6,  fla^=segint(foot6,  direc,  seg6pin,  seg6pout); 
if  (flag —1) 
distc  =  distd; 
end 

[intqtt,  dist6,  flag]=segint(foot6,  direc,  seg6nin,  seg6nout); 
if(flag=  1) 
distc  =  dist6; 
end 
% 

maxdist  =  min(  min(dista,  distb),  distc); 

% 

break; 

end  %this  ends  tripod  1  testing 

./,•«**»****•••*••••*•***•*•***•«••**•••***•*•*»•*•«••*••••*•••••••** 


141 


%  Title:  miLm 
Ktaputs:  (1A3.4.3,6.7,*.9) 

%  Outputs:  [1] 

SPuipaee:  T^MatUbfimction  finds  the  nuuamumMnde  possible  for 
%  AquaRol)al,giv«oanaibitniy<hrectiaaassaiapuL 

H  TtefimctionisfiirtbeZS  cmfiMtpsd. 

H 

fimction  [maxdist]-ind(&otl.foot2/oot34bot4,foat3/oat6,difec,eeabo(itri) 

% 

%  maxclist  •  nsudmuffl  fiMt  movement  in  the  dttectiao  requested 
S  fi)atl:fiMit6  °  (i^y)  oooedinates  of  feet 
%  direc  -  diieGtioo  of  intefest 
%  oenbod  -  (x,y)  coordiiiates  ofthe  center  of  the  body 
%tri>  tripod  mmiberO  (legs  13,3)  or  number  2  (legs  2,4,6) 

% 

iind>'37.3;  Kiimer  radius  of  wotkspsoe 

outrad  =  178.2107;  %  outer  radius  of  woricspace 
% 

seglnin  ~  [36.8686,  -6.8S24];  %  inside  endpoint  positive  segment  #1 
seglnout  -  [160.2049,  -78.0M6];  S  outside  endpoint  of  positive  se^nent  #1 
seglpin  >  [36.8686, 6.8324];  36  inside  endpoint  of  negative  segtneid#! 
seglpout  =  [160.2049, 78.0606];  36  outside  endpoint  of  negative  segment  #1 
36 

seg6nin  » [12.3, -333333];  36  inside  en^iointi^ positive  sepnent# 
seg6nout  [12.3,  -177.7718];  36  outside  endpoint  of  positive  segment  # 
aegjSpin  ~  [24.3686,  -28.3030];  36  inside  endpoint  of  negative  aegmeot  # 
segdiMut  -  [147.7049,  -99.7112];  %  outside  en^int  c£ negative  se^nent  # 

36 

segSnin  -  [-24.3686,  -28.3030];  36  inside  endpoint  of  positive  segment  # 
seg3naut  >  [-147.7049,  -99.7112];  36  outside  endpoint  of  positive  segment  # 
segSpin  •  [-12.3, -33.3333];  36  inside  endpoint  of  negative  segment# 
teg3pout  ^  [-12.3, -177.7718];  36  outside  endpoint  of  negative  segment  # 

36 

seg4oin  -  [-36.8686, 6.8324];  36  inside  ax^int  of  positive  segment  # 
seg4oout  •=  [-160.2049, 78.0606];  36  outside  endpoint  rtf' positive  segment# 
aeg4pin  ==  [-36.8686,  -6.8324];  36  inside  endpoint  of  negative  segment  # 
segtpout  =  [-160.2049,  -78.0606];  36  outside  endpoint  of  negative  segment  # 

36 

seg3nin  ==  [-12.3,-33.3333];  36  inside  endpoint  of  positive  segment  # 
seg3nout  =  [-12.3, 177.7718];  36  outside  endpoint  of  positive  segment  # 
seg3pin  =  [-24.3686, 28.3030];  36 inside en^ioint of negahve segment# 
seg3pout  -  [-147.7049, 99.71 12];  36  outside  endpoint  of  negative  segment  # 

36 

seglnin  =  [24.3686, 28.3030];  36  inside  en^int  of  positive  segnoent# 
seglnout  ~  [147.7049, 99.71 12];  36  outside  endpoint  of  positive  segment  # 
seg2pin  =  [12.3, 33.3333];  36  inside  endpoint  of  negative  segment# 
seglpout  =  [12.3, 177.7718];  36  outside  endpoint  of  negative  segment  # 

36 

flag”0; 
while  (tri  “  0) 

36testfootl  inleg  1  workspace 
11=1; 

[intept,  distl,  flag]=arcint(footl,  direc,  cenbod,  outrad,  seglnout,  seglpout,  il); 
if(flag==l) 
dista  =  distl; 
end 

[intept,  distl,  fla^=arcint(feotl,  direc,  cenbod,  inrad,  seglnin,  seglpin,  ft); 
if(flag=  1) 
dista  =  distl; 
end 

rotate  >=0; 

[intept,  distl,  flag]^gint(footl,  direc,  seglpin,  seglpout,  rotate);  %  CCW  segment 
if(flag=  1) 
dista  =  distl; 
end 

rotate  =>  1; 
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[inicpi.  diitl,  flag]*wgiiil(footl,  dine,  seglnin,  seglnout,  roOteX  %CW  legmeot 
if(fUg“  1) 

(tiiU'diAl; 

cad 

K 

H  test  foots  in  leg  3  woriopace 
ft  =  3; 

[inept,  diets,  fleg]~ercin(foot3,  dine,  cen>od,  outnd,  segSnout,  segSpout,  ft); 
if  (flag  ••  1) 

m  d|gt3^ 

end 

[inept,  distS,  flag]-anint(fi>ot3,  dine,  oen>od,  inad,  segSnin,  segSpin.  ftX 
if  (flag— 1) 
w  dtst3« 
end 

route  *0; 

[inept,  diets,  flag]-*cgin(fi>otS,  dine,  eegSpin,  aegSpout,  rotate),  Si  OCW  ee^nen 
if  (flag—  1) 
s  dist3^ 
end 

route  *  1; 

[inept,  dik),  flag]>segin(foot3,  direc,  eegSnin,  eegSnout.  rotate),  %CW  segonent 
if  (flag  —  1) 
disib  ^  dist3; 
end 
% 

Si  test  foots  in  leg  J  woriespaee 
ft-3; 

[inept,  diets,  flag]>arcint(footS,  diree,  eenbod,  outnd,  segSnout,  segSpout,  ft); 
if  (flag  —  1) 
a  dlSt5^ 
end 

[inept,  diets,  flag]-arcin(footS,  diree,  eenbod,  innd,  segSnin,  eegSpin,  ft), 
if  (flag  —  1) 
dietc^dietS; 
end 

rotate  >0; 

[inept,  dins,  flag]«>eegin(foatS,  direc,  segSpin,  segSpout.  rotate).  Si  CCW  segmen 
if  (flag—  1) 

Hiffr  B  - 

end 

routes  1; 

[inept,  diets,  flag|=segin(ftiotS,  direc,  segSnin,  segSnout,  rotate),  SiCW  segmen 
if(flag—  1) 
dietc  =  distS; 
end 
% 

%  fprinf(nstuff,'  LI  =  SiS.lf.dista); 

%  Q)tinf(nstuff,'  Ls  -  %S.lf,distb); 

Si  4>rinf(n3tuff,'  LS  =SiS.lf,distc), 
msj(dist  min(  min(dista,  distb)  distc); 

% 

break; 

end  Si  this  ends  tripod  0  testing 
Si 

while  (tii  —  2) 

Si  test  foots  in  leg  2  wotkiqiace 
ft  =  2; 

[inept,  diets,  flag]^arcin(fi>ot2,  direc,  eenbod,  outrad,  se^nout,  segSpout  ft), 
if  (flag  —  1) 
dista^distS; 
end 

[inkpt  distS,  flag]=arcin(foot2,  direc,  eenbod,  inrad,  se^nin,  segSpin,  ft); 
if  (flag—  1) 
dista  =  distS; 
end 
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raOte^O; 

(iaicpi,  dikz.  iUig]>Mginl(&>al2,  dirtc,  leglfia,  segZpoiit,  ratiteX  %  CCW  segmeol 
if  (flag— 1) 

cod 

rotate  =  1; 

[iatG{it,dtlt2,flag]>iegjbal(fi>ot2,direc,ieg2iiiii,aeg2iiout.  rotate);  %CW  tegpMitt 
if  (flag  —  1) 

m  dilt2; 

end 

% 

%  teet  fi>at4  in  leg  4  wofiapace 
ft  ”4; 

[iitept,  di(t4,  flag]>arcait(foot4,  direc,  oenbod,  outrad,  teg4iiout,  seg4)X]ut,  ft); 
if  (flag  —  1) 

^stb~diat4; 

end 

[iatqpt,  dist4,  fla^*arciiit(foot4,  direc,  ceabod,  torad,  ieg4ciiii,  seg4piii,  ft); 
if  (flag  —  1) 
distb  >  dist4; 
end 

rotate  “0; 

[intcpt,  diat4,  fla^'=segint(foot4,  direc,  ieg4pin,  leg^iout,  rotate>,  %  CCW  segment 
if  (flag— 1) 

(&tb  =  dist4; 
end 

rotate*"  1; 

(iidcpt,  diat4,  flad-segiiit(foot4,  direc,  seg4niii,  teg4noiit,  rotate);  %  CW  segment 
if  (flag  —  1) 

(ttstb  >•  dist4; 
end 
% 

%test  footd  in  leg  6  woricspace 
ft  “6; 

[intqpt,  distd,  flag]"*arcmt(footd,  direc,  cenbod,  outrad,  segdnout,  segfipout,  ft); 
if  (flag— 1) 
distc«dist6; 
end 

[intcpt,  distd,  fla^=arcint(foat6,  direc,  cenbod,  inrad,  segdnin,  segdpin,  ft); 
if  (flag— 1)  - 
<fistc~dist6; 
end 

rotate  **0; 

[intcpt,  distd,  flag]^segiQl<foot6,  direc,  segSpin,  segSpout,  ratate>,  %CCW  segment 
if  (flag— 1) 
distc  *■  distd; 
end 

rotate™  1; 

[iidcpt,  dist6,  flagl"*segiiit(foot6,  direc,  segdnin,  segdnout,  rotate);  %  CW  segment 
if  (flag— 1) 
distc  "*  distd; 
end 
% 

%  fprintfl[)nstuff,'  L2  ™  %S.lf,dista); 

%  fprintf^^nstufr,'  L4™%5.1f,distb); 

%  ftnintfl’nistufi’,'  L6  =  %3.1f,distcX 
maxdist  ™  iiiin(  min(dista,  distb),  distc); 

% 

break; 

end  %  this  ends  tripod  1  testing 

•/o  •»*•••**♦•••••••••••••••••••••••••*••*•••••*•*••»*•••••••*••••••••••• 

%  Title:  mk-m 

%  Purpose:  Tests  maxdist  routines  at -)-/-?  degree  interv  als. 

•/,*«•*•••«•••*•******»***•**»*•«*•*«••••***»***•••*••**•••*••*•**•••* 

% 

function  mkO 
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% 

ftl-C9«.02,0]; 

fi2>[12.S,83];  %{49.01,84.8S78]; 
fl3>H9.01.84.8878]; 
ft4»[-97.0];  S(-98.02,0]; 
ft5*[-49.01.-84.8878]; 
ft6>[12.3,-83];  %(49.01.-84.8878]; 
cenbod~[0,0]; 

% 

!delniitiiff 

% 

1ri*0; 

^iriatfC^iiituff,'  Tripod  0  -  FOSrnVE\n'); 

■■  ■!■— ■.Tim-.ii.i  .1  T -n  I  r.  \n\o'); 

dtrw  ~  0:90:360 
^iriiitfCinitiiff.'d  »  %4.0f,dinc); 

[m]  =  md(ftl^;ft3jS4JlSjft6,direc,oenbod,tri); 

m 

4iriidfl>iituff,'  iiiax>%S.lfn'^); 
end 
% 

4)riotfCiiistuff,‘\n  Tripod  0  -  NEGA'nVE\n')', 

fordir«c  =  -0:-90:-360 
^jtintfCinstuff.'d  -  %4.0f,direc); 

[m]  >  nid(fll^,ft3^4^4l6,diiec,oenbod,tri); 
m 

^irintf(iiistiiff,'  niax  =  34S.lfn'4n); 
end 
% 

tri  =  2; 

% 

4>rint<(testuff,‘\n\n\o  Tripod  1  •  POSlTlVE\n'); 

^intf(totuff,*~  . .  rnn'.ri.-i  rrTT-w,  „  . . .  .  ~  ~  ~  '  \n\n'); 

for  diTM  0:1:360 
fptintlCWluff.'d  3M.0f,direc); 

[m]  ^  tnd(ftl^4l34l4JtS4t6.dd<ec,cenbod,tri); 
m 

iprintf)>atiiff,'  max>  %S.lf\n'^); 
end 
% 

iptintf(teituff,‘W  Tripod  1  •  NEGA'nVE\n')', 

lprintf(testuflF,'  •  —  — ' — •'  '  •  - — ^-  \n\n'); 

for  direc  =  -0:-l:-360 
iprintf(testuff,'d  -  %4.0f, direc); 

[m]  >  nid(ftl4l24t3^44U4l6.direc,cenbod,tri); 
m 

fpiintfCmstuff,'  max  =  %3.1f\n'^); 
etid 


This  MatUb  file  contains  the  I  -angles 

parameters  used  in  the  tnaxdm,  | 

segmLm,  and  arcinLrc  functions.  - + - +a 

NOTE:  This  is  NOT  an  executable  | 
progranL  |  Wangles 

+Y 

paratns.n[i;  27APR93;  2Scm  foot  only 

These  parameters  were  derived  using  the  arciiit2.m  function  called 
as  shown  using  Leg  ONE  ii^Nits  as  an  exart^le: 

[x,y,<m  =  arcint2(25,0;30,0,0,37.5). 

Leg  ONE  (x,  y):  workqtace  angles  defined  fixxn  (23, 0) 
radius  inner  arc  37.3  cm: 

positive  inner  segment  (36.8686,  6.8324);  30  degrees 
negative  inner  segment  (36.8686,  -6.8324);  330  degree 


I 
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ndiiis  outer  arc  178.2107  cm: 
podtivc  outer  segment  (160.2049,  78.0606>,  30  degrees 
negative  outer  segment  (160.2049,  '78.0606);  330  degrees 

LegTWO()ty):  workspace  angles  defined  from  (12.3,21.6306) 
radius  inner  arc  37.3  cm; 

positive  inner  segment  (12.3, 33  J333);  90  degrees 
negative  inner  segnieat  (24.3686, 28.M30);  30  degrees 
radius  outer  arc  178.2107  cm: 
positive  outer  segment  (12.3, 177.7718);  90  degrees 
negative  outer  se^nent  (147.7049, 99.7112X  30  degrees 

Leg  THREE  (x;y):  woilcgrace  angles  defined  fiom  (-12.3, 21.6306) 
radius  inner  arc  37.3  cm: 

positive  irmer  segment  (-24.3686, 28.3030);  1 30  degrees 
negative  inner  segirent  (-12.3, 33  J333);  M  degrees 
radius  outer  arc  178.2107  cm: 
positive  outer  sepnent  (-147.7049, 99.71 12y,  130  deuces 
negative  outer  segment  (-12.3, 177.7718);  M  depees 

Leg  FOUR  (X,  y);  wotkqisce  angles  defined  fiora  (-23, 0) 
radius  iiaier  arc  37.3  cm: 

positive  inier  segment  (-36.8686, -6.8324);  210  degrees 
negative  irmer  sepnerU  (-36.8686, 6.8324);  130  degrees 
radius  outer  arc  178.2107  cm: 

positive  outer  segment  (-160.2049, -78.0606);  210de9ees 
negative  outer  segmem  (-160.2049, 78.0606y,  130  degrees 

Leg  FIVE  (x,  y):  workspace  angles  defined  fiom  (-12.3, -21.6306) 
ra^us  inner  arc  37.3  cm: 

positive  iimer  segmetd  (-12.3,  -33.3333);  270  degrees 
negative  itmer  sepnent  (-24.3686, -28.3030X  210  degrees 
radius  outer  arc  178.2107  cm: 
positive  outer  segment  (-12.3, -177.7718);  270  degrees 
negative  outer  sequent  (-147.7049,  -99.71 12);  210  degrees 

Leg  SIX  (x,  y);  workspace  angles  defined  fixxn  (12.3,  -21.6306) 
radius  inner  arc  37.3  cm: 

positive  irmer  segment  (24.3686,  -28.3030);  330  degrees 
negative  inner^egmetd  (12.3, -33.3333);  270  degrees 
radius  outer  arc  178.2107  cm: 
positive  outer  segmerd  (147.7049, -99.71 12X  330  degrees 
negative  outer  segmeid  (12.3, -177.7718);  270  degrees 

This  Matlab  file  ootdains  the  [-angles 

parameters  used  in  the  niaxd3.ni,  I 

segint3  jn,  aixl  arcint3  jn  functiom.  - - - +X 

NOTE:  This  is  NOT  an  executable  I 

program.  j+angles 

+Y 

paramsS  jn;  26APR93;  43cm  foot  only 

These  parameters  were  derived  using  the  arcint2.m  fiinction  called 
as  shown  using  Leg  ONE  inputs  as  an  example; 

[x,y,<Ul  =  arcint2(23,0,30,0,0,37.3). 

LegONE(x,y):  workspace  angles  defined  fiom  (45, 0) 
radius  outer  arc  149.27  cm: 

positive  outer  segment  (139.0446, 54.2967);  30  degrees 
negative  outer  segment  (139.0446,  -54.2967);  330  degrees 

LegTWO(x,y):  workspace  angles  defined  fiom  (22.3, 38.9711) 
radius  outer  arc  149.27  cm; 
positive  outer  segmeid  (22.3, 147.5645);  90  degrees 
negative  outer  segment  (116.3446, 93.2678);  30  degrees 


Leg  THREE  (x,  y):  workspace  anglei  defined  from  (-22.3,38.9711) 
ndius  outer  arc  149.27  ana: 

poaittve  outer  segnent  (-116.3444, 93.2678),  130depcei 
negative  outer  segment  (-22.3, 147.3643),  M  degrees 

LegFOUR(]^y):  workspace  angles  defined  from  (-43, 0) 
radius  outer  arc  149.27  cm: 

positive  outer  segment  (-139.0446,  -34.2967),  210  degrees 
negative  outer  segment  (-139.0446, 34.2967)  130  degrees 

LegFIVE(x,y):  workspace  angles  defined  from  (-22.3, -38.9711) 
radius  outer  arc  149.2707  cm: 
positive  outer  segnient  (-22.3,  -147.3643),  270  depees 
negative  outer  segment  (-1 16.3446,  -93.2678),  2 10  degrees 

l-eg  SIX  (x,  y):  workspace  angles  defined  from  (22.3,  -38.971 1) 
radius  outer  arc  149.27  cm: 

positive  outer  segnient  (1 16.3446,  -93.2678);  330  de^ees 
negative  outer  segment  (22.3, -147.3643),  270  degrees 


%  Title:  pltfooLm 
%  Inputs: 

%  Outputs:  pltmetplot 

%  Purpose;  This  Matlab  program  plots  the  progression  ofKNEE2  and  the 
%  FOOT  ofAquaRobot  given  a  known  X  foot  coordinate  and  the 
%  desired  leg  stride. 

% 

clear 

cig 

ck 

Idelpltinet 

boldrtf 

y^obal=-70.7107; 

px  « input(TtesentfootX;  O'. 

% 

34  NOTE:  use  next  line  for  direct  entry  of  next  x-coordinate,  OR 
34  dx  =  input(Desired  foot  X;  *); 

34  NOTE:  use  next  two  lines  to  enter  stepsize  fi'om  known  foot  point 
xin  =  iiq>ut(T)esired  stride:  *); 
dx  -  px  +  xin; 

34 

[elpse,  e,  elx]  =  ellipse(px,  dx); 

34 

Jup  =  D; 

kneel  >°Q; 
knee2  =  Q; 

foot«0; 

for  n  =  l:length(elpse) 
elpse(n,2)  =  elps^n^)  +  yglobal; 

[t0,tl,t2,t3]  ~  invl^l,elpse(n,l),0,elpse(ii.2)), 
[Igcljc24l]°kin(t0,tl,t2.t3); 
knee2=[knee2ja(l,l)  k2(l,3)]; 

foot=[foot;fl(l.l)ft(U)]; 

end 

hip-Ih(l,l)h(U)]; 
kneel=(kl(l,l)kl(l,3)); 
axis((-10  180-10060]) 
plot(hip(l,l)4up(l,2V+c2’) 
hold  on 

title(Elliptical  Foot  Motion*) 
xlabelCAquaRobot  X-coordinates*) 
ylabelCAipiaRobot  Z-coordinates*) 

34 

plol(kiieel(l,l)Jcneel(l,2),'+c3') 
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% 

plol(kiiMa(l.l)J(i>M2(UX'*c87 

plQl(kiicc2(p,  l)JaMe2(pr2X'^ 
end 

plal(k]ice2(n.lXlciiee2(i42X‘xc8^ 

H 

plo«(fb«H(l.l)/ooi(UX'*<^ 

fbtp^Url 

pl^foot(p.l),fi>«<(p>2X'oc6^ 

plot(foat(ii.lX£>ot(n^X‘»^') 

% 

te«(0.3.0.38,'Hip','K0 

teia(0.4.0.S8.'Kr,’ieO 

te3«l(0.2.0.18.’*-st»it','K:’) 

te^O.2,0. 1  S.'x  -  soar. W) 

grid 

meUptt 


%  Title:  fcsetm 
Inputs: 

HOu^mU:  kcItnMt  plot;  ksdau  file 

HPuipoee:  This  MatUbprogrim  plots  the  RESET  potture  for  AqusRobot 
% 

idelks 
.'del  kcR-met 
clear 
bold  off 

sflgle01>0*pi/180;  441egl 

sogle02-60«pi/l^; 
siigleO3-120Vl^:  ^  ^ 

aii^e04>180*pt/180;  44  leg4 
siigle03-240*pi/180;  44  legS 
sagle06-300*pi/180;  44  leg6 
44 

a0=37.3; 

al-20; 

a2=J0; 

a3»l<K); 

44 

snglel^: 

44 

siigle2F>^.4«pi/180; 
sngle2B~66.4*pi/l  80; 

44 

angle3F=lS6.4*pi/180; 

siigle3B=-136.4*pi/180; 

44 

T00=[oos(aiigle01)  -sinCtiigleOI)  0  0,sin(*ngle01)  cot(tagle01)0  0;0  OlOfiOOl]; 
T01-[oos(anglel)  -siii(aoglel)  0  aO;siii(siiglel)  c^anglel)  0  0;0  0  1 0;0  0  0  1]; 
HIP“T0O»T01; 

T12={co«(angle2F)  -tiii(siigle2F)  0  al;0  0  1 0;-iiii(angle2F)  -cos(aug;e2F)  0  0;0  0  0  1]; 
KNEE1«T00*T01*T12; 

T23~[cos(aiigle3F)  •«in(angle3F)  0  a2»iii(angle3F)  cos(angle3F)  0  0;0  0  1  0;0  0  0  1]; 
KNEE2-TOO»T01»T12*T23; 

T34-(l  0  0  a3;0  I  0  0;0  0  I  0;0  0  0  IJ; 
diuy  tci 

FOOT1=TOO*T01*T12*T23*T34; 
diary  off 
% 

T00»[cos(angle02)  -sin(angle02)  0  0-,sin(aiigle02)  cos(angle02)  0  0;0  0  1  0;0  0  0  1); 
TO  ’  >[cot(anglel)  -siiKangiel)  0  aO;siii(snglel)  c^inglel)  0  0;0  0  1  0;0  0  0  1]; 
HIP=T0O*T01; 
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T12-(«><aBgte2F)  •«ia(aiigle3F)  0  al;0  0  1  0;-cii<aiigk2F)  -c<M(iiigU2F)  0  0;0  0  0  1]; 
KNEEl-T00TX)l*Ti2; 

T23>[ciM(aagle3F)  -cu^angleSF)  0  a2;siii(ai«le3F)  cai(tii«k3F)  0  0;?  0 1 0;0  0  01]; 
ICNEE2-TOOTX»1*T12*T23; 

T34-{1 0  0  s3;0  1  0  0;0  0  1 0;0  0  0  1]; 
dUtyks 

FOOT2-TOOnt)l*T12*T23*T34; 
diaiy  off 
% 

T00-(cat(aiig|e03)  •«m(aogle03)  0  0,siii(aiigle03)  c(M(«iigle03)  0  0;0  0  1  0;0  0  0  1]; 
T01-(CM(aiigiel)  -*tii(aiiglel)  0  a0,siii(aiigicl)  c(a(ai^l)  0  0;0  0  1 0-,0  0  0  1]; 
HIP-T0O*T01; 

T12~[coi(ingle2B)  -<in(aiigle2B)  0  <1;0  0  1 0;-<iii(aiig|e2B)  -ooi(aiig)£2B)  0  0;0  0  0  1]; 
KNEE1=T1)0»T01*T12; 

T23-{oat(aiigle3B)  •<iii(tngle3B)  0  a2;siii(augle3B)  ooi(aii«le3B)  0  0;0  0  1  0;0  0  0  1]; 
KNEE2«T0O*T01*T12*T23; 

T34-[l  0  0  a3;0  1  0  0;0  0  1 0;0  0  0  1]; 
diaiy  kt 

FOOT3-TOO*T01*T12*T23*T34; 
diaiy  off 
% 

T00=[oos(aiigle04)  -finCaiigleOd)  0  O^inCangleOd)  ca6(aiig|e04)  0  0;0  0 1  0;0  0  0  1]; 
T01=[ooa(aiiglel)  ■siii(aiiglel)  0  a0'4iii(aiig|el)  c^ai^el)  0  0;0  0  1 0;0  0  0  1]; 
HIP-TOO»T01; 

T12=(oos(angle2B)  •tio(aa^2B)  0al;00 1  0;-sin(aiigie2B)  -cos(angle2B)  0  0;0  0  0  1]; 
KNEE1=T00*T01*T12; 

T23»[co«(angle3B)  -ain(aiigle3B)  0  a2;siii(aiigle3B)  ooa(angle3B)  0  0;0  0  1  0;0  0  0  1]; 
KNEE2=T00»T01»T12*T23; 

T34>(1  0  0  a3;0  1  0  0;0  0  1  0;0  0  0  1]; 
diaiy  ks 

FOOT4=TOO»T01»T12»T23*T34; 
diaiy  off 
% 

T00=[ooa(aiigle05)  -siii(aiigie05)  0  0;siii(aiig]e03)  caa(anglcOS)  0  0;0  0  1  0;0  0  0  1]; 
T01’°[ooa(aiiglel)  -aiii(aiigtcl)  0  aO;siii(aiiglel)  o^anglel)  0  0;0  0  1 0;0  0  0  1]; 
HlP-noO^TOl; 

T12=>(cos(aogle2B)  •oiii(aiigk2B)  0  al;0  0  1  0;-siii(aiigle2B)  -cos(aagle2B)  0  0;0  0  0  1]; 
KNEEl=TO0*T01*T12; 

T23=[Gas(aiigle3B)  -sin(angle3B)  0  a2;si>i(angle3B)  CQs(angle3B)  0  0;0  0  1  0;0  0  0  1]; 
KNEE2»TOO*T01»T12*T23; 

T34=tl  0  0  a3;0  1  0  0;0  0  1 0;0  0  0  1]; 
diary  ks 

FCX3T3=T00»T01*T12*T23*T34; 
diaiy  off 
% 

T00»[cos(aiigle06)  -sin(angle06)  0  0;siii(aiigle06)  cos(aiigle06)  0  0;0  0  1 0;0  0  0  1]; 

TOl  =|cos<anglel)  -siii(angk:l)  0  aO',siii(anglel)  c^anglel)  0  0;0  0  1  0;0  0  0  1]; 
HIP=TOO*TOl; 

T12=(cos(angle2F)  -sin(angie2F)  0  al;0  0  1  0;-5iii(angle2F)  •cos(angIe2F)  0  0;0  0  0  1]; 
KNEEI=T00»T0l*T12; 

T23=[cos(angle3F)  -siii(aiigle3F)  0  a2;ain(angle3F)  cos(angIe3F)  0  0;0  0  1 0;0  0  0  1]; 
KNEE2=T00»T01*T12*T23; 

T34-(I  0  0  a3;0  I  0 0;0  0  1  0;0  0 0  1]; 
diary  ks 

Fo6t6=T00*T01»T12*T23*T34; 
diarv'  off 
% 

x=fFOOTl(l,4)  FOOT2(1.4)  FOOT3(l,4)  FOOT4<l,4)  FOOT5(l,4)  FOOT6(l,4)]; 

y=[FOOTl(2,4)  FOOT2(2,4)  F<X)T3(2,4)  FCX)T4(2,4)  FOOT5(2,4)  FOOT6(2,4)]; 

axis([-200  200  -200  200]) 

plol(x,y,'o') 

hold 

x=0; 

y=0; 

plot(x,y,’+g') 

xlabelfX  Foot  Coordinates  (cm)') 
ylabelOY  Foot  Coordinates  (cm)*) 
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ti!lcCAi]uIlobat  RESET  Poatuic’) 
ffid 

tfejaCFOOrn 

glexi(TOOTr) 

Ctata^ 

meUkcR 

HTtoMATLAB  program  finifa  the  mtewfrtinmrf  the 
%  deioed  dtracitoa  ^travel  and  the  woffapece. 

% 

%  Wiittea  by  Chuck  Scfaue.  29DEC92 

HiLm 

% 

%  Notes:  1.  Aquerobot  positive  angle/rotstioo  is  CCW. 
%  2.  Matleb  positive  ai^le/TOtation  is  CW. 

%  3.  Leg  One 

% 

clear 

cic 

dg 

FUg-0; 

%  Declarations 
RX1I-1S7.6844; 

Ryii>76.6034; 

Rxp-157.6«44; 

Ryp--76.6054; 


%  Define  line  (pas_leg_wo(kspace_liniit) 
tbetapin~>30; 
tbetair>thetapin*pi/180; 
xlinep-23+(12.3*cos(lheUp))-, 
ylinep- 12.3  *sin(thetap); 


%  Define  line  (neg;_leg_wofkspace_limit) 

thetaniiF*30; 

tbetaii-thetanin*pi/l  80; 

xlinea»23-i<12.3*oaa(thetan)); 

ylinen-12.3*ain(thetan); 


%  Define  arc  (max  workspace  limit)  with  radius  178.2107 
% 

cmix=Q; 

r-178; 

x=(160:0.3:178];%actuaUy  137.6844  to  178.2107 
form=l;lengtli(x) 
y=sqtl(r^-x(:jn)^); 
caux»[cnia]Qx(:mi)  y]; 
end 

x=in8:-0.3;160];%actuaUy  137.6844  to  178.2107 
forn=l:length(x) 
y=-iqrt(r^-<:4i)^); 
cmax>(cniax;x(.,n)  y]; 
end 

%  Define  arc  (inin_wofkq>ace_limit)  with  radius  37.3 
% 

cfflin^Q; 

r=37.3; 

X-P6.9733:0.1;37.3]; 
forin=l:lenglh(x) 
y=sqit(r^-x(:jn)^); 
aam=‘lcnuB;x(:jn)  y]; 
end 

x=p7.3:-0.1:36.9733]; 

forn»l:leag|h(x) 

y=-sqrt(r^.x(:^)^); 


cniiii-(cniiii;x(:4>)  yj; 
end 

%  Define  ny  (fbotpoint  and  deaded  ducction  of  travel) 
tlieUitf*inputCDeeirad_Difectiaa  of  travel:  *); 
if  (thetain^-M)  1  (theuin— -270) 
tlietar--89.3*]^180; 
ebeif(thetain«--90)  |  (thetain>°^70) 
thetaf~89.3*pi/180; 
elM 

thetar-4betain*pi/180; 

end 

xray^inputCEnter  knovn  footpoint,  X  ooordinale:  *); 
yrayoiqHitOEalcr  known  footpoint,  Y  coordinate:  y. 


%  Determine  if  new  footpoint  is  within  poe  &  veg  leg  limits 
distL|in^tinen-yray)*c^tbetanHxliiMa  xray)*sin(thetan); 
distLpp^linep-yray)*oos(thetap)-(xlinep-xray)*tio(thetapX 


if  (((distLpipK))  I  (distLpn°-0))  &  ((distLpp<0)  |  (distLpp>»0))) 
%  Determine  wbether  there  is  a  positive  intenectioo 
thetapos^thetar-thetap; 

if  (thetapoe==0)  |  (theli^los=^)  |  (thetapos=-pi) 
disp(l)esired_Diiection  and  Positive_Leg_Lii^  are  parallel.') 
xintpoe-xray; 
yintpos-yray, 
else 

b^xlinep*sin(thetap)Xyiinep*cos(tbetap)); 

a=^xray*sin(thetar)>(jTay*CM(thetar)); 

xto()K'<^thetap)*a)+{cias(thetar)*h); 

ytop><sin(thetar)*b)-(sin(thet^)*a); 

bottooi~s^(thetap4hetar); 

xintpos«'Xtop/bottoni; 

yintpas=ytop/bottom; 

%  Determine  if  the  ray  intenecis  the  pos  line  segment 
Lp^xiiiiep*oos(thetap)^yIinep*sin(thetap))', 
Rp>‘(Rxp*cos(thetap]^Ryp*s^thetap)); 
segtest-(xin4>os*cas(thetap)4-yiii4x)s*nn(thetap)); 
if  ((Lp<segtest)  &  (Rp>segtest))  %  if  true,  intersection 
%  Detomine'ifthe  orientation  is  correct 
p2°(xiidpos*cos(thelar)*-yintpos*sin(thetar)); 
pl=(xraj^cos(thetar)+^y*sin(thetar)); 
ifp2>pl  %  correct  orientation 
Flag-1; 
end 
end 
end 


%  Determine  whether  there  is  a  negative  inteisection 
thetaneg-thelar-thetan; 

if  (thetaneg--0)  |  (thetaneg— pi)  |  (tbetaneg— -pi) 
dispCDesiied_Directioo  and  Negative_Leg_Limit  are  parallel.') 
xintneg-aray; 
yintneg-yray, 
else 

b-(xlinen*sin(thetan))-(yiinen*cos(tbetan)); 

a-(xray*sin(thetar)>i^y*eos(thetar)); 

xtop-(-cos(thetan)*a>Kc^th^)*b); 

ytof>-(sin(thetar)'%)-(siii(thetan)*a); 

bottom-sin(thetanfiietar); 

xintneg-xtop/bottom; 

yintneg-ytop/bottom; 

%  Determine  if  the  ray  intersects  the  neg  line  segment 
Ln=(xlinen*cos(thetan>i-ylinen*$in(thetan)); 
Rn=(Rxn*cos(tbetan)+Ryn*sin(the^)); 
segtat-(xintneg*cos(thetan>t-yintneg*sin(thetan)); 
if  ((Ln<seglest)  &  (Rn>seg'est))  %  if  true,  intersection 


151 


*/»  Ddcnnine  if  the  orientation  is  conect 
p2=(xintneg*cas(thetar>>7intDeg:*sin(thetar)); 
p  1  “(xniy*cos(theUr)+yray*$in(thetar)); 
ifp2>pl  ^  cofTcct  orientation 
Flagyl; 
end 
end 
end 

%  Calculate  the  line  segnients 
posint^Q; 

X'^xlinep:  1:160; 
forp=l;length(x) 
y=(-0.5774*x(:,p)>^-14.4338; 
posint={posintpt(;,p)  y], 
e^ 

negint-O; 
x^xlinen:  1:160; 
for  q=l:lenglh(x) 
y={0.5T7»!c(:,«i)>14.4338; 
neginf=[negint;x(:,q)  y]; 
end 

%  Draw  the  orientation  of  the  ray 

ray=D; 

if  Flagyl  %  positive  intersection  is  good 
if  xin4>os<xray 
x=xintpos:  1  :xray; 
else 

x=xray:  1  :xintpos; 
end 

m=(yray-yin^>osy(xray-xintpos); 

b=yray-{m*xray); 

elseifFlag=-l  %  negative  intersection  is  good 
if  xintneg<xray 
x=xintneg:l:xray; 
else 

x=xray:l:xintneg; 

end 

ni=(yintneg-yray)/(xintneg-xray); 

b=yTay-(m*xray); 

else 

Flag=0;  %  no  intersection  with  line  segments 


%  -M-t-M-++iiisert  intersection  with  arcs  here+++ 


end 

for  g=l:length(x) 
y“{m*x(:,g))+b; 
ray=(ray,x(:,g)  y]; 
end 

%  Plot  the  results 
axis([0  200  -100  100]) 
axisCsquare') 

plot(posint(:,  l),posint(:,2),'g) 
hold  on 

plot(negint(:,  l),negint(:^),'g;) 
plot(cmin(:,l),cmin(:,2),'^ 
plol(cmax(:,l),cmax(:^),’gf) 
grid 

plot(xray,yTay,'+c5') 

ifFlag==l 

plot(xintpos,yintpos,'or^ 

elseifFlag=-l 
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plot(xinlDeg,yiotaeg,'otO 

else 

FUg-0; 

text(0.25,0.2,'Ray  ietenects  only  srcs.'.'sc') 
end 

plo«(r«y(;,l)^y(:A'*>') 

titleCInlenectioQ  of  Directioa  ind  Wotkspace') 
xUbel(?<  coordinates,  (cm)') 
ylabe](T  coordinates,  (cm)') 
bold  off 

else  %  new  fiootpoiot  not  inside  pos&neg  leg  limits 
%  Calculate  the  line  se^nents 
posint=Q; 
x=xlinep:l:160; 
farp-l:length(x) 
y-<-0.5774»x(:4)))+14.4338; 
posint-[posiiit^:  j>)  y]; 
end 

negint-Q; 
x>xliaen:  1:160; 
for  q=l:lenglli(x) 
y=(0.577*x(:,q)>14.4338; 
negint=-(negiiit;x(:,q)  y]; 
end 

%  -T^-  III.  - 

%  Plot  the  results 
axis([0  200  -100  100]) 

plot(posiiit(;,l),posiiit(:,2),'g') 
hold  on 

plot(oeginl(:,l)jiegint(;,2),'g) 

plot(cmin(:,lXcmin(:,2X'g') 

plot(cmax(:,lXcmax(:,2X'g') 

grid 

plot(may,yriy,'+c5') 

title(lnteisection  of  Direction  and  Workspace*) 
xlabelCX  coordinates,  (cm)*) 
yiabel(T  coordinates,  (cm)5 
text(0.2S,0.2,Tootpoint  not  inside  iiniits.','sc') 
hold  off 
end 


%  Title:  segintm 
%li^ts:  (1,2,3,4,5) 

%Oiiqxits:  [1,2,3] 

%  Purpose:  This  Matlab  function  determines  the  intersection  of  a  ray 
%  and  a  line  segment 

% 

function  [intcptdistjlag]  =  segint(foot  direc,  inseg,  outseg,  vector) 

% 

%  intcpt  =  (x,y)  coordinates  of  intercept 

%  dist  =  distance  between  foot  and  intercepts 

%flag=  1  ifintercept  found;  flag  =  0ifno  intercept  found 

%  foot  =  (x,y)  foot  components 

%direc  =  dhectirm  of  interest 

%  inseg/outseg  =  endpoints  of  segment 

%  vector  =  1,  line  segment  is  clockwise 

%  vector  =  0,  line  segment  is  counter-clockwise 

% 

limit  =  215;  %  maximum  possible  distance 
flag  =  0; 

direc  =  direc*pi/180; 

% 

%  Find  theta  of  segment 

thetaseg  =  atan2(outseg(l,2)  -  inseg(l,2),  outseg(l,l)  -  inseg(l,l)); 
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% 

%  Oetenniiie  if  foot  &Us  on  the  line  tegomit 

distLp  « ( ((faot(l^>imeg(l^))*co«(tbeUse«)}  -  ((foo<(l.l>-in*eg(l.l))**«n(tl>e***eg)) ); 

diitL|>  -  rnuniKHirtF  fX 

if(diittp“0) 

flag  =  1;  %  fiiot  is  oo  tine  segment;  quit 

Hfd  s  limit; 

intcpt^  [foot(l.l)4bot(U)]; 
elseif  ( (i&tL{>  >  0)  &  (vector  =  0)  ) 
flag  >  >1;  %foot  is  outside  wotlcspace;  quit 
dist-linut; 
intcpt^Q; 

elseif  ( (distLp  <  0)  &  (vector  •=  0)  ) 
flag  =  0;  foot  is  inside  workspace;  find  intersection 
elseif  ( (distLp  <  0)  &  (vector  “  1)  ) 
flag  =  -1;  %foot  is  outside  workspace;  quit 
dist  =  Iiinit; 
intcpt°  Q; 

elseif  ( (distLp  >  0)  &  (vector  =  1)  ) 
flag  °  0;  %  foot  is  inside  workspace;  find  intersection 
end 
% 

if  (flag  “=  0) 
tbetadif  =  direc  -  thetaseg; 

if  (theUutif  0)  &  (Ihetadif—  pi)  &  (Iheladif  •-=  -pi) 

b  =  (inseg(l,l)*sin(thetaseg))  -  (inaeg(l,2)*cos(thdaseg)); 
a  =  (foot(l,l)*sin(direc))  -  (foot(l,2)»cos(direc)); 
xtop  -  (•cos(thetaseg)*a)  +  (coe(direc)*b); 
ytop  -  (sin(direc)*b)  -  (sin(thetaseg)*a); 
bottom  >=  si^tbetaseg  -  dittt); 
xint  =  xtop/bottom; 
yint  =  ytop/bottom; 
iotcpt  =  (xint,  yint]; 

%  Oetennine  if  the  ray  intersects  the  line  segment 

CCW  =  (inseg(l,l)*c^tbetaseg)  +  inseg(14)*sin(*l'*fo“8)); 

CW  ”  (outseg(l,l)*cas(thetaseg)  +  outseg(l,2)*siii(thetase^); 
segtest  =  (xint*cos(thetaseg)  +  yint*sm(thetaseg)); 
if  ((CCW  <”  segtest)  &  (CW  >=  segtest))  %  if  true,  intersection 
%  Delemiine  if  the  orientation  is  correct 
p2  =  (xint*cos(diiec)  +  yiin*sin(ditec)); 
pi  -=  (foot(l,l)»eos(ditec)  +  foot(l,2)*sin(direc)); 
ifp2>pl  %  correct  orientation 
flag  =  1; 

dist  =  sqrt((yint  -  foot(l,2))^  +  (xint  -  foot(l,l))^X 
else 

flag  =  0; 
end 
end 
end 
end 
% 

if(  (flag  = -1)1  (flag  =  0)) 
flag  =  0; 
dist  limit; 
intcpt  =  D; 
end 

%  Title:  seginlLm 
%  Inputs;  (1,23,4) 

%  Outputs:  [1,2,3] 

%PuTpose:  This  Matlabfiinction  determines  the  intersecticm  of  a  ray 
%  and  a  line  segment 

% 

fimction  [intcptdist41agl  =  segintl(foot  direc,  inseg.  outseg) 

% 
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%  intqpl  •  (x,y)  coorduiaic*  of  iolcrcqpt 
%  diit  diflam  bc^wccn  foot  and  mIerctpU 
%flag-  1  if  ndcfcc^  found;  flag -0  if  no  intcrcqit  found 
%foot  -  (x,y)foat  companenU 
%  dirac  ~  directioa  of  intcrcA 
%  inae^outaeg  -  endpointa  of  legmcnt 
% 

flag“0; 

direc  ~  direc*pi/180; 

% 

%  Find  thcla  of  acgmeoi 

thetaseg  -  ataa2(outseg(l,2)  -  iiifeg(l,2),  outtegfl.l)  -  inseg(l,l)) 
% 

%  Determine  if  the  foot  is  on  the  line  legmcol 

CCW  -  (inseg(l,l)*co*(t>>e(>*eg)  +  iiiaeg(l^)*iiii(lheta*eg)) 

CW  ■=  (outseg(l,l)*coe(tlietaacg)  +  out»eg(l,2)*aiii(lhetaseg)) 
Kglert  -  (foot(l.l)*ooc(thetaaeg)  +  foot(l,2)*iiii(thetaieg)) 
piusc 

if  ((CCW  <=  legleft)  ft  (CW  >-  aegtest))  %  if  Hue,  foot  is  on  line 
flag  -  1; 
dist  =  0; 
inlcpt-foot; 
end 

if  (flag  -=  0) 
thetadif  -  direc  •  thetaseg; 

if  (thetadif  -=>  0)  ft  (thetadif pi)  ft  (thetadif--  -pi) 
b  =  (inseg(l,l)»sin(lhetaaeg))  -  (inseg(l,2)*cos(thetaseg)); 
a  =  (foot(l,l)*sin(diree))  -  (fo<d(l,2)*coe(duec)); 
xtop  -  (-c»e(thetaseg)*a)  +  (cos(direc)*b); 
ytop  -  (sin(direc)*b)  •  (si^thctafeg)*a); 
bottom  -  sin(thetaseg  •  direc); 
xint  -  xiop/bottom; 
yint  -  ytop/bottom; 
intcpt  -  [xint,  yint]; 

a  Oeterinine  if  the  ray  intersects  the  line  segment 
CCW  -  (inseg(l,l)*cos(thetaseg)  +  inseg(l,2)*sin(thetaseg)); 
CW  =  (oulseg(l,l)»eos(thetaseg)  +  outseg(l,2)*$in(thetaseg)); 
segtest  “  (xim*coi(thetaseg)  +  yint*sin(theuseg)); 
if  ((CCW  <=  segtest)  ft  (CW  >=  segtest))  %  if  true,  intersection 
%  Detennine-if  the  orientation  is  correct 
p2  -  (xint*oos(dircc)  +  yint*sin(direc)); 
pi  -  (foot(l,l)*cos(direc)  +  fo<^l,2)*sin(direc)); 
ifp2>pl  correct  orientation 
flag  -  1; 

dist  -  sqit((yiiit  -  foot(l,2))^  +  (idiit  -  foot(l,l))^); 
else 

flag-0; 

end 

end 

end 

end 

if  (flag  —  1) 
flag  -  0; 
dist  -  []; 
intcpt”  [); 
end 


%  Title:  stabintm 
•/.Inputs:  (1,23.4,5.6.7.8,9,10) 

•/•Outpub:  [13.3,4] 

•/«  Purpose:  This  Matlab  function  detennines  the  intersection  of  a  ray 
•/.  and  a  line  segment  in  one  Tripod. 

•/,•••»•••••••*•*•••••••**•••••••••••••••••••••••••••••*•••••' 

% 

fonction  [xint,yint,disUlag]  -  sUbint(Tjx,ry,rtheta,ax,ay.bx,by,cx,cy) 

•/. 
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ST'  tripod  number,  0  or  1 

S  ax/y.  bx/y,  cx/y  '  wlccted  tripod  foot  pocitiaai 

%  rx/^  '  ray  convonentt 

S  rih^  '  direction  of  inlercat 

S  xint/yint »  segment  intercepis 

%  diet 'dirtance  between  ix/ry  and  xint/yint 

Sflag'  1  ifinteroeptfoum^  flag 'Oifnointeroept  found 

%  lega  '  legl/2,  lej^  '  leg3/4,  lege  =  legS/6 

% 

S  convert  degrees  to  radians  and 

%  oonect  for  differences  in  positive  angle  oonvenlion  used 

%  in  computer  and  used  on  AquaRobot;  AquaRobot  positive 

SangleisCW 

itheta  '  •(itheta*pi/180); 

% 

%  correct  that  -t-y  axis  in  Matlab  is  -y  axis  in  AquaRobot 

ty«-ry; 

ay  =  -ay. 

by- -by, 

ey  =  -cy. 

% 

llag'O; 

ifT='0 

%  test  segment  1-3  (lega-legc) 

% 

S  Find  theta  of  segment 
thetaseg-atan2(ay-cy,ax-cx); 

S 

tfaetadif-rtfaeta-thetases 
if  (thetadif“H))  |  (1hetadif=pi)  ]  (thetadifo=-pi) 
dispOparaller)  S  desired  direction  and  line  segment  are  parallel 
else 

b'(cx*sin(thetaseg))-<<=y*coa(<l>‘t*^<S})'> 

a'(rx*sin<ttbeta))-(iy*oos(rtheta)); 

xtop=<-coa(thetaseg)*a>+<cos(rtheta)*b); 

ytop-<sin(rtheta)*bKtin(<lKtaseg)*a); 

bottom-sin(thetaseg4theta); 

xlSint-xtop/bottom; 

ylSint-ytop/bottom; 

S  Determine  if  the  ray  intersects  the  line  segment 
legcpt'(cx*oos(thetaseg>+cy*sin(thetaseg))', 
leg^it-<ax*cos(thetaseg)+ay*sin(thetaseg)); 
seglest=<xl3int*oos(thetaseg)+yl3int*sin(thetaseg)); 
if  ((legcpt<-segtest)  &  (segtest<=legapt))  %  if  true,  intersection 
%  Determine  if  the  orientation  is  correct 
p2=<xl5int*cos(tlheta)+ylJint*sin(rtheta)); 
pi  =(rx*cos(rth^>Hy*sin(ttheta)); 
ifp2>pl  S  correct  orientation 
dist=s<pt((y  1 3int-ryy^+(xl  5int-rxy'2); 
xint  -  xlSint; 

yint ' -yl  Sint;  %  recorrect  y  axis 
end 
end 
end 

- -r-,r 

%  test  segment  S-3  (legc-legb) 

% 

%  Find  theta  of  segment 
thetaseg=atan2(cy-by,cx-bx); 

% 

thetadi^'Ttheta-thetaseg; 
if  (thetadif='0)  |  (thetadifo=pi)  |  (thetadif='-pi) 
dispCpaiallel')  %  desired  direction  and  line  segment  are  parallel 
else 

b'(bx*sin(thetaseg)Hby*cos(thetaseg)); 

a=(rx*sin(ttheta))-{ry*c»s(tthela)); 
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xlap=(-oa«(theUseg)*s>Kcc«(itbeU)*b); 

ytop-(iiii(rtfaeUi)*b><«in(<h^»«*»)*>X 

botioin-iai(tfactaseg4theUX 

xS3iot>*xlo|Vboaam; 

y33int-ytop/boaatn; 

%  Dctenniiie  if  the  ray  intcTMcIi  the  line  Mgmeitt 
legc|)t=(cx*co«(thelaseg>H7*iiii(th(Useg)); 
legb|it*^x*ca«(tbeUseg>+t^r*siii(thetiseg)X 
te8fat-(x33iiit*ooi(tfaeUieg>+y53int*siii(theUieg))-, 
if  ((legcpP>-ieg|at)  A  (s^tat>>leg{b|it))  34  if  true,  intersection 
34  Determine  if  the  orienlatioo  it  correct 
p2-(x53int*0(M(rtbeU>^3int*sin(itheU)); 
pl“(rx*coe(flh^)+ty^iin(rtheU)X 
ifp2^1  34  oonect  orientation 
dirt-sqrt((y53int-fy)^+(x53int-rx)^X 
xint  •  x33int; 

yint -y33int;  %  recorrect  y  axis 
end 
end 
end 

34  test  segment  3-1  (lesb-lcgt^) 

% 

34  Find  theta  of  segment 
thetase^atan2(ay-by,ax-bx); 

34 

thetadifntheta-thetaseg; 
if  (ihetadif“0)  1  (thetadif==pi)  1  (thetadif=^pi) 
diqtOparaUel')  34  desired  difection  and  line  segment  are  parallel 
else 

b=(bx*sin(thetaseg))-(by*OQs(thetaseg)); 

a=Krx*sin(ftheta)><>y*c^ithina)); 

xta|>~(-co^tfaetaseg)*a>+<cas(i1hM)*b); 

ytop»<tin(itheu)'bH*>t>(t°<'‘**‘S)**)i 

bottoai-sin(thetaseg4thetaX 
x3  lint^xtop/bottom; 
y3  lint^-ytop/bottom; 

34  Detennine  if  the  ray  inteisects  the  line  segment 
legapt=(ax*oos<thetaseg)+ay*sin(thetaseg)); 
legbpt>(bx*oos(thetaseg>'-l^'*sin(thetaseg)); 
segtest°(x31int*oos(thetaseg>+y31int*sin(tlietaseg)); 
if  ((legb^<-segtest)  &  (segtest<=legapt))  34  if  true,  intersection 
34  Detennine  if  the  orientation  is  correct 
p2=(x3 1  int*oos(tthetaX*-y3 1  int*sin(ttheta)X 
pl=(rx*cos(rtheta>+Ty*shi(rtheta)); 
ifp2>pl  34  correct  orientation 
dist=sqrt((y31int-ty)^2+<x31int-rxy^); 
xint  =  x31int; 

yint  =  •y31iiit;  34  reconect  y  axis 
end 
end 
end 
% 

elseifT=l 

34  test  segment  2-6  (lega>legc) 

34 

%  Find  theta  of  segment 
thetaseg=atan2(ay<y,ax-cx); 

% 

thetadi^Ttheta-thetaseg; 
if  (thetadif=0)  |  (thetadil==pi)  |  (thetadif=-pi) 
dispOtarallel')  34  desired  direction  and  line  segment  are  parallel 
else 

b^cx*sin(thetas!^g))-(cy*cos(lhetaseg)); 

a=(rx*sin(rtheta)Xry*cos(t1beta)); 

xt<^>=(-<»s(thetaseg)*a)+<cos(ttlieta)*b); 


ytop-(iai(itlica)*bHnn(<l>«>MX)**); 

bottam-niCItNluegitlieU); 

x26iaMitop/boltaiii; 

y26ait-ytop/boaoin; 

H  Ddenniiie  if  the  fay  imcrsecti  the  line  segment 
lega|iHax*Mi(lhel«Mg>vay*siii(1hnaseg)y. 
legc^t-(cx*oat(1hetascgyK:y*siii(tlieSas^); 
segtertF^)d6int*ooe(thetas^>t-y26int*siii(thetaseg))', 
if((legcpt<-teglcet)  *  ($egl«<=leg*|)t))  %  if  true,  intersection 
%  Detencine  ifthe  orienUtiaa  it  cocrect 
p2=(x36int*cot(itheta>t-y26int*sin(ftheta)); 
pl^nc*cae(itheU)+iy*tin(itheta)); 
ifp2^1  %  conect  orientatioa 
dut=virt((^6at-ryy^-^(x26int-rxy^); 
xint  x26int; 

yint » -y26iiit;  %  icoofiect  y  axis 
eod 
end 
end 
H 

%test  se^nent  6-4  (legc-le^) 

% 

%  Find  theta  of  segment 
1heUseg=atan2(cy-by,cx-bx)', 

% 

thetadifftbeu-thetase^ 
if  (tbetadif=0)  |  (theUdif=pi)  |  (thetadit"<^i) 
disp(^>artUer)  %  desired  dire^on  and  line  segment  are  parallel 
elae 

b-(bx*siii(thetaae^><by*cos(tbetaaeg)); 

a-(rx»sio(ffheta)Kfy*«>a(rthe‘»))‘. 

xtop~<-oo^theta^*a>t<cot(ttliAa)*b); 

ytop=(sin(itheta)^><*i“(‘**‘*“*8)**); 

bottoi]F°sin(thetasegitheta); 

x64iBt^v^op/bottoaii 

y64inr~yto|i/bottoni; 

%  Detennine  if  the  ray  intersects  the  line  se^nent 
legcpr><cx*cos(tbetaseg>+cy*$in(thetaaeg)); 
leg|bpt-<bx*cos(thetaseg>+hy*sin(thetas^)-, 
segtest=(x64int*cos(thetase^y64int*sin(thetaseg)); 
if  ((legept<=segtest)  &  (segtest<=legibpC))  %  if  true,  intersection 
%  Determine  ifthe  orientation  is  cotrect 
p2=(x64int*cos(rtheU>+y64iijt*sin(rtheta)); 
pl“(t’t*®os(rti>^)*ty**“'(rtif***)); 
ifp2^>l  %  collect  orientation 
dist=sqrt((y64int-ryy^+(x64iiit-rxy^); 
xint  =  x64iiit; 

yint  =  -y64in^  %  recorrect  y  axis 
end 
end 
end 
% 

%  - - 

%  test  segment  4-2  (legh-lega) 

% 

%  Find  theta  of  segment 
thetaseg=atan2(by-8y,bx-ax); 

% 

thetadifntheta-thetase^ 
if  (thetadif=0)  |  (thetadif=pi)  |  (thetadif=-pi) 
dispCparaller)  %  desired  diiection  and  line  segment  are  parallel 
else 

b={ax*sin(thetaseg))-(ay*eos(ii*etaseg)); 

a=(rx*sin(itheta))-(ry*cos(rtheta)); 

xtop=(-c<^theta^)*a)+(cos(rth^)*b); 

ytop=(sin(rtheta)*b>-(s®(lhetaseg)*a); 


batlaimin(thetaa^<tbeu); 

x42iflt-xlop/boaom: 

y42iDt*7lo|)/bo(taai; 

%  Dttamiiie  iftfae  imy  mtaurti  tht  line  ngnent 
le8*|it<K*x*G0i(tfaelaie^^iiy*sii>(tfaclaicg)); 
kgbpt-^x*c<n(tfatU»eg)  I  t^Hn(thc«aiicK)X 
seglc)t-<x42iiit*cai(tl>ctaseg>^2iat*<in(theUseg)>, 
if  ((kgi|)t<-ie8lat)  ft  (scgtcsK-Icgijpt))  %iftnie.iiiunecuaii 
%  Ddenniiie  if  the  OficnUtioa  is  ootiecl 
p2^x42int*cas(itheU>'742int*siii(ftheU)); 
pl>(ix*oai(ithctt>tTy*siii(itl>eU)); 
ifp2>pl  %  ooRcct  orienUtiao 

dist=4qrt((y42iiil-ryy^+(x42ini-ncy^X 
xint  -  x42int 

yint  > -y42inl;  %  leccmct  y  axis 

Old 

end 

end 

% 

else 

disp(lncortect  Tripod  munbcrO 
end 

ifdist=“n 

flag-0; 

else 

flag-1; 

end 

%  Title:  stable  jn 
Sli^uts:  (U3,4,5.6,7,8) 

%  Outputs:  [1^] 

%Puipase:  This  Matlabflmction  calculates  the  stability  of  a  Tripod. 

% 

fiinction  [flagjm]-stable(CBx.CBy,legax,legay,legbx,legby,legcx,legcy) 
•A  CBJe^legb,legc  ocienled  CCW  so  S  is  positive  when  CB  is  inside. 

% 

*A  Hag’  indicates  degee  of  stability 
% 'em' is  the  stability  margin 
%  iega-iegl/2.  legl^leg3/4,  legc-iegS/6 
xl  -  CBx; 

yl  -  -CBy,  %t-y  axis  in  Matlab  is  -y  axis  in  AquaRobot 
x2-legax; 

y2  =  'legay;  %+y  axis  in  Matlab  is  -y  axis  in  AquaRobot 
x3-teghx; 

y3  -  -legby,  %+y  axis  in  Matlab  is  -y  axis  in  AquaRobot 
x4  =  legcx; 

y4  =  -legcy;  %+y  axis  in  Matlab  is  -y  axis  in  AquaRobot 
% 

U  =  S<pt((x3-x2r2+(y3-y2r2); 

U  =  S<pt((x4-x3r2+(y4-y3r2); 

LI  =  S<pt((x2-x4r2+(y2.y4r2); 

% 

51  =  ((x2-xl)*(y4-yl)-(x4-xl)*(y2-yl)y2; 

52  =  ((x4-xl)»(y3-yl><x3-xl)*(y4-yl)y2; 

53  =  ((x3-xl)*(y2-ylHx2-xl)*(y3-yl)>2; 

% 

if  (S1>0)  ft  (S2>0)  ft  (S3>0) 
flag-1;  %  The  new  tripod  is  stable, 
elseif  (S1<0)  |  (S2<0)  |  (S3<0) 
flag--l;  %  The  new  tripod  is  unstable, 
else 

flag-0;  %  The  new  tripod  is  nuuginally  stable 
end 
% 

H1-2»S1/L1; 
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H2-2*S2/U; 

H3-2*S3/U; 

% 

atf’naiaSHimmjy. 

K  Title:  sUrtin 

^Outputs:  kcS-met  plot;  ks  data  file 

%  Puipote:  This  Matlab  program  piols  the  START  pasture  for  AquaRobot 

^  »**»*«»»•••*«*•••••*»*•••••••••••••••*•••••••••••••••••••••»••»•••••• 

% 

Idelks 

IdeikcSjiiet 

clear 

boMoff 

% 

aiigle01^*pi/180;  Hlegl 
aii^e02=60*pi/180;  %leg2 
angle03>120*pi/180;  %  leg3 
aiigle04=I80*pi/180;  %  leg4 
aiigle05>240*pi/180;  %  leg5 
aiigle06=300*pi/180;  %  leg6 
% 

a0»37.3; 

al^O; 

a2=50: 

a3«100; 

% 

angle!  >  0; 

% 

angle2F  =  .33.8383*pi/180; 
angle2B  =  35.8583»pi/180; 

% 

angle3P  •  I23.8383*pi/180; 
angIe3B  « •123.8S8S*pi/180; 

% 

TOO  °°  (cos(angle01)  •<in(angie01)  0  0;siii(angle01)  cos(aagle01)  0  0;0  0  1  0;0  0  01]; 

TO!  =  [coefanglel)  -sinfanglel)  0  aO;sin(angiel)  c^ai^el)  0  0;0  0  1  0:0  0  0  !]; 

HIP  =  T0O*T0!; 

T12  =  (cos(angle2F)  •sin(angle2F)  0  aT,0  0  1  0;'Sin(angle2F)  -cos(angIe2F)  0  0:0  0  0  1): 
KNEE!  =  TOO»TO!»T12: 

T23  =  {cc<(angle3F)  -siii(angle3F)  0  a3'^iii(angle3F)  co((aDg!e3F)  0  0;0  0  !  0;0  0  0  !]; 
KNEE2  =  T00»T0!*T!2*T23; 

T34  =  [!  0  0  a3;0  !  00:00  !0;000!]; 
diary  ks 

FOOT!  =TOO»TO!*T!2»T23»T34; 
diary  off 
% 

TOO  =  {cas(angle02)  •sm(angie02)  0  0'4in(angle02)  cos(angle02)0  0;0  0  1 0;0  0  0  !]; 

TO!  =  [cos(angIe!)  -sio(angle!)  0  aO:sin(angle!)  c^angle!)  0  0;0  0  !  0:0  0  0  !]; 
HIP-T00»T0!; 

T!2  =  [cos(angIe2F)  -siti(angle2F)  0  a!:0  0  1  0;-siii(angIe2F)  -cos(angle2F)  0  0,0  0  0  !]; 
KNEE!  =TOO»TO!*T!2: 

T23  =  [cos(angle3F)  -siii(angle3F)  0  a2:sin(angle3F)  cos(angle3F)  0  0;0  0  !  0;0  0  0  !]; 
KNEE2  =  TOO»TO!*T!2*T23; 

T34  -=  (1 0  0  a3;0  !  0  0;0  0  1 0,0  0  0  !]; 
diary  ks 

FOOT2  =  TOO‘TO!*T!2*T23»T34; 
diary 
% 

TOO  =  [cos(angle03)  ■sin(angle03)  0  0:sin(angle03)  cos(angle03)  0  0;0  0  !  0:0  0  0  !]; 

TO!  =  [cos(anglel)  -sin(angle!)  0  aO:sin(ai]gle!)  c^angle!)  0  0:0  0  !  0;0  0  0!]; 

HIP  =  TOO*TO!; 

T!2  =  [cos(angle2B)  -5in(angle2B)  0  a!:0  0  !  0;-5in(angle2B)  -C03(angle2B)  0  0;0  0  0 ! J; 
KNEE!  =TOO‘TO!»T!2; 

T23  =  [cos(angle3B)  -sin(angle3B)  0  a2:sin(angIe3B)  cos(angIe3B)  0  0;0  0  !  0;0  0  0  !]; 
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KNEE2  -  T00*TD1»T12‘T23; 

T34  -  [1  0  0  a3;0  1  0  0;0  0  1  (HO  0  0  1]; 
diary  ks 

foots  -  T00*T01*T12*T23*T34; 
diary 
% 

TOO  -  [oatCaiigMM)  -fm(aiiglc04)  0  0-,iin(aiig|c04)  oat(angle04)  0  0;0  0  1 0;0  0  0  1]; 

TOl  •  [oai(aiiglcl)  -MKanglel)  0  aO,iii>(angicl)  c^aqglel)  0  0,0  0  1  0;0  0  0  1]; 
HIP-T0O*T01; 

T12  >  [ooa(aiigle2B)  -fiii(aiigle2B)  0  al;0  0  1  0:-tiB(tagle2B)  •cat(aiigle2B}  0  0;0  0  0  1]; 
KNEEl  -  T00*T01*T12; 

T23  >  [oof(aiigl^B)  -tiii(aiigle3B)  0  a2;sin(aogle3B)  coa(ang|fcSB)  0  0;0  0  1  0;0  0  01]; 
KNEE2  -  T0O»T01*T12*T23; 

T34  -  [1  0  0  a3;0  1 0  0;0  0  1  0;0  0  0  1]; 
diary  ks 

FOOT4  -  T0O*T01»T12»T23»T34; 
diary  off 
% 

TOO  >  [oot(angie0S)  -siii(aiigie05)  0  0'4in(aiislc03)  oos(aiig|e03)  0  0;0  0  1 0;0  0  0  1]; 

TOl  =  [cosCanglel)  -tiii(anglel)  0  a0;sii)(aiiglel)  c^anglel)  0  0;0  0  1 0;0  0  0  1]; 
HIP-T0O*T01; 

112=°  [coi(angte2B)  -siii(afig|e2B)  0  al;0  0  1  0;-siii(angle2B)  -cot(angIe2B)  0  0;0  0  0  1], 
KNEEl  “T0O»T01»T12; 

T23  ~  [oos(angle3B)  -ainCanglcSB)  0  a2'4in(aiigle3B)  cos(ang)e3B)  0  0;0  0  1  0;0  0  0  1]; 
KNEE2  -  TOO»T01*T12‘T23; 

T34  -  (1 0  0  a3;0  1  0  0;0  0  1  0:0  0  0  1]; 
diary  ks 

FOOTS  -  T0O*T01»T12‘T23*T34; 
diary  off 
% 

TOO  =  (caa(aiigteOd)  -sinCangleOd)  0  0'4iii(ang|c06)  cos(aiigle06)  0  0;0  0  1  0;0  001]; 
TOl  >  [oos(aiiglel)  -tifi(anglel) 0  aO-,siii(aiigtel)  c^anglel) 0  0;0  0  1 0;0  0  0  1]; 
HIP-TOO'TOl; 

T12  -  (oos(aiigle2F)  -sin(angle2F)  0  al;0  0  1  0;-sii>(aiigle2F)  -ooi(sngle2F)  0  0;0  0  0  1]; 
KNEEl -TOO»T01»T12; 

T23  “  [oot(aiigie3F)  -fiii(angle3F)  0  a2;sin(angle3F)  ooi(aiigle3F)  0  0;0  0  1  0;0  0  0  1]; 
KNEE2  -  TOO*T01»T12»T23; 

T34  -  [1  0  0  a3;0  1  0  0;0  0  1  0;0  0  0  1]; 
diary  ks 

FOOT6  -  T00*T01»T12»T23*T34; 
diary  off 
% 

X  -  [FOOTl(l,4)  F<X)T2(1,4)  FOOT3(l,4)  FOOT4(l.4)  FCX)T5(1,4)  FOOT6(l,4)]; 

y  -  [FOOTl(2,4)  FOOT2(2.4)  FOOT3(2.4)  F<X)T4(2,4)  FOOT5(2,4)  FOOT6(2.4)]; 

axis([-200  200-200  200]) 

plot(x,y,'o’) 

hold 

x«0; 

y-0; 

plo«(x,y,’+g^ 

xlabel(^  Foot  Coordinates  (cm)') 
ylabel^  Foot  Coordinates  (cm)T 
titleCAquaRobot  START  Posture') 
grid 

gtextfFOOTl') 

gtexl^OOT2^ 

glexlCBodyCaitetO 

roeUkcS 


Si  Title;  ucwlinkl.in 
Si  Inputs: 

Si  Outputs;  c4.inetplot 

Si  Purpose;  This  Mstlab  program  plots  the  AVAILABLE  woric^jace  of 
Si  the  HIP  ( Joint  1)  to  KNEEl  (Joint  2)  LINK  1  of  the  AquaRobot. 

Si 
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!deio4jiiet 

inglcO-OVlSO; 

angk2-0*i>i/180; 

aO-37.5; 

al-20; 

% 

»lvec“|]; 

fac  logleW-MVlSOyO  Vl>0);(60*pi/180) 

7BO*[ooi(aBgleO)  -«in(angl«0)  0  0; 
siii(aa{jcO)  cai(aiigleO)  0  0; 

0010; 

0001]; 

% 

T01>[ca6(aiiglel)  -<iii(aii8lel)  0  aO; 
aii(aiiglel)  ca((anglel)  0  0; 

0010; 

0001]; 

H 

T12>[coi(aii^e2)  -siii(aiig|e2)  0  al; 

00  10; 

-aiii(an^e2)  -ca<(ang)e2)  0  0; 

00  01]; 

% 

UNK1=TB0*T01*T12; 
alvec«[alvec;LINKl(1.4)  UNK1(2.4)]; 
end 

axis((0  60-30  30]) 

plot(alvec(:.lX*lve<^:.2)) 

grid 

title(71ip  Workspace*) 
xlabelCX  coordinate,  cm*) 
ylabel(T  coordinate,  cm^ 
gtexlCHIP') 
gtextC-60  degrees') 
glextCOO  degrees') 
g|ext(lCN£El') 
metao4 

•/,*«****«**««««*««***«*«**«««***M**»«***«*****M«*««*«****«m***i 

%  Title:  uc\vitnk2jn 
%  Inputs: 

%Oinpuls:  cSjnetplot 

%  Purpose:  This  MtUlab  propam  plots  the  AVAILABLE  work^Mce  of 
%  the  KNEEl  (Joint  2)  to  KNEE2  (Joint  3)  LINK2  of  the  AtpiaRobot 

^M*******.....*.***..*..*..****.***...,*.*.**.***.,*.**.*...****. 

% 

!del  c3.niet 
% 

angIeO^*pi/lSO; 

anglel=0*pi/180; 

angle3»0*pi/l*0; 

a0=37.5; 

al=20; 

a2=50; 

% 

a2vec-Q; 

forangle2=(-73.4»pi/l*0):(5»pi/180):(106.6*pi/180) 

TBO=[cDs(an^eO)  ^^(angleO)  0  0; 
sio(angleO)  cos(angleO)  0  0; 

0010; 

0001]; 

% 

T01>[cos(anglel)  -sin(anglel)  0  aO; 
sin(anglel)  cos(anglel)  0  0; 

0  010; 

0001]; 

% 
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T12-[ca«(aiigte2)  -«iii(uig|e2)  0  al; 

0  010; 

-8iii(angle2)  -oat(angle2)  0  0; 

0001]; 

% 

T23*[eo((aiigle3)  -cin(aiigle3)  0  a2; 
tin(aiigk3)  ooi(angl^)  0  0; 

0010; 

0001]; 

% 

LINK2=TB0»T01*T12*T23; 
a2vcc-‘[a2vec-XINK2(l,4)  LINK2(3.4)]; 
cod 

axi^EOnO-dOM]) 

P>ol(a2vec(:,lXa2v»c(:^)) 

grid 

tiUcCKneel  Wockipace') 
xlabelCX  coordmate,  cal') 
ylabeIOZ  coordinate,  cal') 
glextCKNEEl') 
glext^.4  degrees') 
g|textC-106.^  degees') 
glext(1CNEE2') 
metacS 


%  Title:  uowliiik3.ni 
%Ii^Hds: 

%Ovaputs:  o6jnetpIot 

%  Purpose:  This  Matlab  program  plots  the  AVAILABLE  workspace  of 
%  theKNEE2(Joint3)toFOOT(Joiiit4)LINK3oftheAquaRot>oL 

% 

!del  c6.inet 
% 

anglcOK)*pi/180; 

anglel=0*pi/180; 

angle2=0*pi/180; 

aO=37.3; 

al=20; 

a2=50; 

a3-100; 

a3vec=Q; 

foraiigle3=(-23.6*pi/180):(5»pi/180):(156-4*P'/180) 

TB0=[cas(an^e0)  -$iii(angleO)  0  0; 
sin(sngleO)  cos(angleO)  0  0; 

0  010; 

0001]; 

% 

T01='[cos(anglel)  -sin(anglel)  0  aO; 
siii(angiel)  co^anglel)  0  0; 

00  10; 

0001]; 

% 

T12=[cos(angIe2)  -sin(angle2)  0  al; 

0010; 

-sin(sng)e2)  -cos(angle2)  0  0; 

0001]; 

% 

T23=[cos(angle3)  -sin(aiigle3)  0  a2; 
sin(aiigle3)  cos(angIe3)  0  0; 

0010; 

0001]; 

% 

T34«[l  0  0  a3; 

0100; 
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0010; 

0001]; 

% 

LINK3-TB0*T01‘T12»T23*T34; 

»3vec-[»3v«aJNK3(l,4)  UNK3(3,4)]; 

«od 

uii([0  230-125  123]) 

plo«(«3vec(:.l)^vec(:^)) 

grul 

tiUcCKiiee2  WoilcspacO 

xUbelCX  oooniaiate,  cal’) 
yUbei^  ooacdinate,  cm*) 
gtext(icNEE2') 
gtexl^3.6  degnt^ 
g|extC-lS6.4  degrees') 
glexl(TOOT) 
meUcd 

%  Title:  w2btniisjii 
%  Inputs:  (1A3,4.S.6.7.8.9) 

%  Outputs:  [1] 

34  Purpose:  This  fimctiaa  truisfomis  world  oaordioates  to  body  coordinates. 

e  ••««««««««*•••«««•««««««««««•«««««••••««••«***»•••«««•«•••***•«•«••*• 

34 

fimction  [body]  >  w2btrans(phi,  theta,  psi,  xtrans,  ytrans,  ztnns,  wx,  wy,  wz) 

34 

34phi-rotatioaof{B}  about  the  Xw-axis; 

34  theta  -  rotatioa  of  {B}  about  the  Yw-axis; 

34  psi  rotatioa  ci  (B)  about  the  Zw-axis; 

34  xtrans  >  X-axis  translation  of  {B}  with  respect  to  {W}; 

34  ytrans  ~  Y-axis  translatioa  of  (B)  with  respect  to  {W}; 

34zlrans  =  Z-axisttaaxlatiaoof{B}  with  respect  to  (W]; 

%  wx  =  known  Xworld  coordinate; 

34  wy  -  known  Yworld  coordinate; 

34  wz  «  known  Zworld  coordinate; 

34 

34  construct  the  translatioo  vector,  the  body  vector,  and  the  rotatioa  matrix 
trans  =  [xtrans,  ytrans,  ztrans]'; 
world  =  [wx,  wy.-vtfz,  1]'; 

rotate  =  [cos(psi)*cos(tfaeta)  oos(psi)*sin(tbeta)*sin(phi)-siii(psi)*cos(phi)  cos(psi)*siii(theta)*cos(piu>fsin(psi)*sin(phi); 
siii(psi)*c^tb^)  sin(pti)*$i^thets)*sin(phi)+cot(psi)*cot(phi)  sin(psi)*sin(th^)*cos(phi)-cos(psi)*sin(phi); 
-sin(th^)  c^tb^)*sin(phi)  cos(th^)*cos0>hi)]; 

34 

34  build  the  body-to-world  homogeneous  matrix 
BWT  =  [rebate  trans;0  001]; 

% 

34  find  the  resultant  4X1  vector  of  equivalent  body  coordinates 
Tbodv  =  inv(BWT)*world; 

34 

34  strip  off  the  (x,y,z)  coordinates 

body  =  [Tbody(l.l),  Tbody(2,lX  Tbody(3,l)]; 


34  Title:  work.m 
3o  Inputs: 

34  Outputs:  woriemet  plot 

•/•Purpose:  This  Matl^  program  draws  the  UNCONSTRAINED  work^Mce  for 
34  AquaRoboL 

•/. 

!del  woriemet 

cig 

cic 

clear 

hold  off 
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^oAnict  body  with  radius  37  3  cm 
c-Q; 

1-37.5; 

*=(-37.3:0.5:37.31; 

fi)rin-l:kiiglh(x) 

y-sqrt(r^-x(:4n)^); 

c-(cjt(:jn)y]; 

end 

*-p7:-0.3:-37.3]; 

foriiPl:leiiglfa(x) 

y»-sqit(i^-x(:ji)^); 

C“(c-^:4>)y]; 

cad 

AxiiCsquafe') 

Axia(I~200  200  -200  200]) 

plot(c(:.l),c(:4)) 

grid 

hold 


%  constnict  legl  2D  workspace 
<*=0; 

x-37.5;0.5;200; 
forp=l:leiigtb(x) 
y=(1.73205‘x(:j))>64.95 19; 
d=‘{<tx(:,piy]; 
end 

plol(d(:,lX‘«(:.2Vg') 

% 

c=(l; 

*-37.3:0.3:200; 
for  q-l:lenglh(x) 
y-(.1.73205‘x(:.q)>+^.93 19; 
e“[e.x(:.q)y]; 

end 

ploKe(:,lX«<:.2V8r) 

% 

%  show  legl 

Ll-0; 

x=37.3:0.3:178.2107; 

forq=l:lengtb(x> 

y-(0»x(:,q)>+O; 

U=[Ll;x(:,q)y); 

end 

plot(Ll(:,l),Ll(:,2X'b’) 

% 

%showfootl 

x-178.2107; 

y=0; 

p!ot(x,y,'oc3') 

%  construct  Ieg4  2D  workspace 

x=-180:0.3:-37.5; 
forr=l:Iength(x) 
y=(-1.73203*x(:^))-64.95 19; 
f=[nx(:j)y]; 
end 

plot(fl::,l)J[:^XW 

% 

8=0; 

x=-200:0.5:-37.5; 

fors=l:length(x) 

y=(1.73205*x(:^))+64.9519; 

g=[g;x(:^)y]; 

end 

plot(g(:,lXg(;.2X’gO 

% 
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%]howleg4 

u-a; 

x--37.5:-0.5:-178.2l07; 
for  q*=l:lcngtli(x) 
y-<0*x(:,q))+<); 

L4=(L4;x(:,q)  y]; 

cod 

pIo«a4(;,lXL4(:.2X'b’) 

% 

%  ihow  foot4 
x=-178.2107; 
y^; 

plot(x,y,'oc5') 

r -  I  -- . 

%  construct  leg2  2D  workspace 

h=a; 

x=18.75;0.5;200, 
fort=l:length(x) 
y=<0*x(-..t)>32.476. 
h=[hyt(;.t)  y]; 

end 

plol(h(:.l)Ji(:,2X'g^ 

% 

k=D; 

x=18.75:-0.5>200; 
foru=l:lenglh(x) 
y=(1.73205*x(:.u)><4.9519; 
k=[k;x(:,u)  yj; 
end 

plot(k(:,l)Jc(:^V80 

% 

%  show  Ieg2 

L2-D; 

x=18.75.0.J:89.1054; 
for  q=l:lenglh(x) 
y=(-1.73025»x<:,q))+0; 
U=(Up<:.q)y]; 
end 

plot(U(:,lXL2(:.2).'b') 

% 

%sbowfoot2 

)C=89.1054; 

y=-134.335; 

plot(x,y,'oc5') 

%  construct  leg}  2D  work^>ace 
1=0; 

x=-18.75:O.J:200; 
for  v=l  :Iength(xt 
y={1.73205*x(;,v))+«4,9519, 

l=[lpc(;,v)  y]. 

end 

plot(l(:,lXl(:.2X'g^ 

% 

m=[l; 

x=-18.75:-0.5:-200; 

forz=l:length(x) 

y=(0*x(:^)>+32.476; 

ni=[npc(:^)y]; 

end 

plot(in(:,l)^:,2),'g^ 

% 

%  show  legs 

L3=D; 

x=-18.75:-0.5:-89.1054; 

forq=l:length(x) 

y=(-1.73025»x(:,q))+0-, 


L5=[L5yt(:.q)y]; 

cad 

plo«(U(.,lXL5(:.2).t)') 

% 

%ihow  foots 
)c--89.10S4; 
y- 154.335; 
plot(x,y.'oc5') 

%  coBstruct  le^  20  woricspace 
1=0; 

x=-18.75:0.5:200; 

forv=l:leiiglh(x) 

y-(-1.73205»x(:.v)><4-9519; 

l=0;x(:.v)y]; 

end 

plo«(l(:,lXl(:.2Vrt 

% 

ni=0; 

ic=-18.75:-0.5;-200; 
forr=l;lengtli(x) 
y=(0*x(:^))-32.476; 
m“{in;x(;;z)  y]; 
end 

plot(ni(:,lX™(:,2Vg') 

% 

%showleg3 

L3-D; 

x=-18.75;-0.5:-89.1054; 

forq=l:length(x) 

y=Kl-73025»x(;,q)>+0; 

U=(Upc(:.q)y]; 

end 

plot(U(:,l),U(:,2),'b') 

% 

%  show  foots 
x=-89.1054; 
y=-154.335; 
plot(x,y,'oc5') 

%  construct  leg6  20  workspace 
1=0; 

x=18.75;-0.5:-200; 
forv=l;lengtli(x) 
r=(-1.73205»x(;,v))+64.95 19, 
l=[l;x(:,v)y]; 
end 

plot(l(:,lXI(:.2),‘gO 

% 

M=0; 

x=18.75;0.5;200; 

forz=l:length(x) 

y=(0»x(:,z))+32.476; 

m=[m;x(:,2)y]; 

end 

plot(m(:.l),ni(:.2),'g') 

% 

%sbow  Ieg6 

L6=0; 

x=18.75:0.5;89.1054; 
for  q=l:Iength(x) 
y=(l-73025*x(:,q)>+0; 
L6=[L6;x(:,q)  y]; 
end 

plot(L6<;,l),L6(:,2),'b') 

% 

%  show  foot6 
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x-89.1054; 

y=154.335; 

plol(x.y;oc5') 


%  label  plot 

titlefAquaRobot  Uncooitniiied  Leg  Wafk^Mce*) 

xlabclCX  coordinate,  cm*) 

ylabel^  coordinate,  omO 

glexiCLeg  One*) 

metawofk 


APPENDIX  C: 


"C^”  PROGRAM  LISTING 


This  appeadix  contains  the  real-time  gait  planning  code  written  in  the  ANSI 
language  using  the  ATT  version  3.1  compiler.  These  code  modules  are  ready  for 
implementation  in  the  AquaRobot  simulator  (Silicon  Graphics  Personal  Iris).  These  code 
modules,  less  the  graphics  portions,  may  also  be  used  in  the  AquaRobot  control  computer 
(NEC  486-based  Personal  Computer)  with  slight  modifications  because  of  compiler 
differences. 
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//  FILENAME:  AbBotiy.C 

//  PURPOSE:  InvlanenUtiao  of  the  AqiurobotBody  class 
//  CX>NTAINS:  initializes  the  body  fotm 

#iiiclude  ’AbBotfy.H’ 

AquarobotBody: :  AquarobotBodyO 

{ 

body_list  >  new  matrix(7,4,0.0);  //  each  row  is  a  body  point  (x.y;t) 
// the  first  (0  row)  is  the  body's 
//  physical  center,  the  rest  are  the  six 
//  peunts  r^the  body 

H_inatrix  =  new  matitx(4,4,0.0);  //  the  body's  H-niatrix 

//  the  body  design  is  constructed 
body_list->val(0,0)  *  0.;  body_list->val(0,l)  =  0.; 
body  list->val(0J)  *■  0.;  body_list->val(0,3)  =  1.; 
body~list.>val{l,0)  =  37.5;  body_list->val(l.l)  =  0.; 
body  litt->val(l,2)  =  0.;  body_list->val(l3)  !•; 
body“list->val(2,0)  ■=  18.75;  body_list->val(2.1)  =  32.48; 
body_list->val(2,2)  •  0.;  body_list->vsl(23)  =  l-l 
body_list->val{3,0)  -  -18.75;  body_list->val(3,l)  =  32.48; 
body_tist->val(3,2)  *  0.;  body_list->val(33)  =  !•; 
body_list->val(4.0)  =  -37.5;  body_list->^(4,l)  “=  0.; 
body  liat->vsl(4,2)  *■  0.;  body  list->val(43)  =  1.; 
body jist->val(5,0)  =  -18.75;  body_list->val(5,l)  =  -32.48; 
body  list->val(5,2)  =  0.;  body_list->val(5,3)  =1.; 
bodyjist->val(6.0)  =  18.75;  body_list->val(6.1)  =  -32.48; 
body  list->val(6,2)  *  0.;  body_list->val(6,3)  =  1.; 

// defines  the  initial  location  of  the  body  using  the 
// H-matrix  the  inputs  to  the  fijnetion  are: 

//  (azirmith,  elevation,  roll,  x,  y,  z) 

//  H_matrix->HomogeneousTransfbrni(0.,0.,0.,0.,0.,  -54.1819); 
H_matrix->HooiogeneousTransfomi(0.,0.,0.,0.,0., -70.7107); 

// moves  the  body  to  the  initial  location  desired 
body_list->TrinsfotmBodyList(*H_tnatrix,*body_lis*); 


void  AquarobotBody.'.'MovelncrementaKdouble  delaz,  double  delel, 
double  deirol,  double  delx,double  dely,  double  delz) 

{ 

double  az,  el,  to,  X,  y,  z; 
az  -  azimuth  delaz; 
el  =  elevation  -t-  delel; 
ro  =  roll  +  deirol; 

X  =  body_list->val(0,0)  +  delx; 
y  =  body_list->val(0,l)  +  dely, 
z  =  body_list->val(0,2)  +  del^ 

//  changes  only  are  used  since  body_Iist  is  at  current  position 
H_inatrix->HoinogeneousTransfonn(delaz,  delel,  deirol,  delx,  dely,  delz); 
body_list->TransfonnBodyLisl(*H_matrix,*body_list); 

//  puts  all  info  in  H_fflatrix 

H_inalrix->HoinogeneousTransfonii(az,el,ropc,y,z); 

} 

//»«*.«***•*»**•*•***••••*«••**•*•***•**•«**••••*•*••••*•«•** 

//  FILENAME:  AbBody.H 

//  PURPOSE :  Declaration  of  AquarobotBody  class 

//  Subclass  of  RigidBody  class 

//*»•****»**•*•••••••*••**••••»•*••***•••**«<•••*••*•*•«••**• 
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#ifedrfH_AQUAROBOTBODY 
Mefioe  H_AQUAROBOTBODY 


^iDclude  <itdio.h> 
^include  "linlclH" 
^include  "AbRigidH” 
#include''Liiik.H'' 
#incliide  "MathxMy.H” 


class  AiiuarobotBody ;  public  RigidBody 

{ 

pubUc; 

matrix  *body_list;  //  defines  the  size  of  the  body  using  coordinates 

matrix  *H_mxtrix;  //  defines  the  location  for  body 
//  using  azimuth,  elevation,  roll,  x,  y,  z 
double  azimuth,  elevation,  roll; 

AquarobotBodyO;  //constructor 

void  Movelncreinental(doubie,double,  double,  double,  double,  doubteX 


}; 


#endif 

//  FILENAME;  AbLeg.C 
//  PURPOSE:  In^lementation  of  Aqual^eg  class 
//  CONTAINS:  AquaLegQ 
//  Ioitialize(AquaLeg&,  AquaiobotBody&) 

//  TakePicture(Cainera&,  AquaLegA) 

//  Moveincreniental  (Aqualeg&,  delul,delia2,delta3) 

#include  "AbLegH" 

//  FUNCTION:  -AquaLegQ 
//  PURPOSE:  destructor  of  AquaLeg  class 

AquaLeg:  :-AquaLegO 

{ 

delete  linkO; 
delete  linkl; 
delete  linlc2; 
delete  Iink3; 

} 

//  FUNCTION:  AquaLeg 
//PURPOSE;  constructor  of  AquaLeg  class 
//RETURNS;  AquaLeg  class  with  values 

//»**••*•*•**••••**•**••*>••***«*••••*•«••••«•••***•*•' 

AquaLeg:: AquaLeg! AquarobotBody  &body,  double  angle) 

{ 

motion_complete_flag  =  1; 

SetLegAttachmemAngle(angle); 
linkO  =  new  LinkO; 
linkl  =new  Linkl; 

Iink2  =  new  Link2; 
link3  =  new  Link3; 

//  initial  link  values  initialized 
matrix  temp; 

tefnp.Ui.'dateTMatrix(GetLegAttachmentAngleQ,0.,O.,O.); 
temp  =  ‘b  )dy.H_matrix  •  temp; 

linkO-' RotateLink(&tenq>  ,linkO->GetInboardJointAngleO); 


liiikl->RoUteUiik(liiikO->H_matrix,  linkl-XjeUnboardJotntAogleO); 
liiik2->Rot»tfl  inkQinlf  l->H_in»trixJiiilc2-><jetlnboafdJotatAngleO>, 
liiik3^IloUteLiiik(liiik2*>H_nwtnxJiiik3->GetIiiboard  <oiotAi]£ieO); 

} 

//  nJNCnON:  MovelncronenUl 

//PURPOSE:  calculate  the  new  link  vilucs  as  a  kg  rotites 

//RETURNS;  rotated  link'k  new  values 

U  ««««*««««**«««*•««••«««•««*«•««•«««««««•««•••••«««««•«•••••« 

void  Ai]uaLeg;;MoveIncTemental(Aquarol)otBody  dtbody  double  dehal, 

double  detta2,  doubk 

deltas) 

{ 

double  b; 

//  set  all  limit  flags  to  zero 
Iinlcl>>SetMotionLiniitFlag(0); 
link2->SetMotionLiniitFlag(0); 
liok3->SetMationlinutFIag(0); 

// temp  matrix  adds  in  the  T_matrix  needed  for  the  physical 
// attadanent  of  the  leg  to  the  body 
matrix  temp; 

ten9.UpdateTMatiix(GetLegAttadimentAngleO,0.,0.,O.X 

temp  =  *body.H_matrix  *  temp; 


b  =  deltal  +  linkO->GetInboardJointAngIeO; 

liokO->SetInboardJointAogle(b)-, 

liokO>>RotateLink(&temp,tinkO->GetInboardJoiatAngleO); 


b  =  detta2  +  linkl->GetInboardJointAogleO; 

Iinkl->SetInboardJointAngle(b); 

liakl->RotateLiolc(ltnkO'>H_matrix,linlcl->GetlnboardIomtAn^eO); 
b  =  dehaS  +  this->link2->GetInboardIointAngleO; 

Uiik2->SetInboardJointAngle(b); 

Unk2->Rotate(linlc  l->H_matrix,link2->GetInboar(UoiiitAngleO)l 

b  =  deltas  +  this->linlrS>>GetInboardJointAngleO; 
linkS>>S<tIoboardJointAngle(b); 

IiiikS->RoUteLink(linlc2'>H_matrix,linkS->GetInboardJointAngleO); 

//themotion_coniplete_flagissetto  1  ifthe 
//  nK>tion_linut_flags  on  all  legs  are  not  set 

SetMotionConq>leteFtag(!(thisc>linkl->GetMotionLiniitFlagO  S 

th's->link2-><3etMotionLimitFlagO  i 

this->link3->GtrtMotionLiinitFlagO)); 


//  prints  the  status  of  the  requested  motion  and  prints  which 
//  link's  motion_liinit_flag  was  set  ('f  any), 
if  (GetMotionCompleteFlagO  —  0)  { 

printiCMotion  Not  Completedln"); 
if(linkl-><jetMotioiiLiinitFlagO  “  1) 

printfClink  1  limit  exceededln”); 
if  Gink2->GetMotionLimitFUgO  =  1) 

ptiiitfl["link  2  limit  exceedetbn"); 
if  (linkS->GetMotionLiiiutFlagO  1) 

printfl^linlc  3  limit  exceededln”); 

} 

//  else  printfCMotion  completedin'O; 


} 


//**».***«.*.•*••**•**•**»•**.*••***«••» 

// FILENAME;  AbLeg.H 

//  PURPOSE:  Declarations  for  AquaLeg  class 
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// 

//  COMMENTS:  Definitioo  o(  Aqualxg  class  and  fimctions  that 
//  apply  to  this  class 

#ifodefH_AQUALEG 
^define  H_AQUALEG 

#inchide  ’AbBody.H' 

#incliide  "Unk.H” 

^include  TinkOH” 
ttinclude  ’Lmkl.H'' 
ttinclude  'Luik2.H' 
tHinclude  ’LinlO.H' 

class  AquaLeg 

{ 

public: 

LinkO  *liiikO;  //  a  LinkO  class  is  instantiated 
Linkl  *linkl;  II  a  Linkl  class  is  instantiated 
Link2  *liiik2;  //  a  LinkS  class  is  instantiated 
Link)  *link3;  //  a  Link3  class  is  instantiated 
int  inotion_coaq)lete_flac // the  flag  is  set  to  1  ifthe  motion 
//  was  cooqtleted  without  reaching  any 
//  link  limits. 

int  leg_suppa(t_flae  // the  flag  is  set  to  1  ifthe  leg 
//  is  on  the  floor  (z  -  0). 

double  leg_attad>ment_angle;  //  the  angle  off  of  leg  one  where  the 
// leg  is  attached  to  the  body 

double  previous_foot_x_coord;  //  saves  last  foot  position 
double  previous_foot_y_coord; 
double  pievious_foot_z_coord; 

AquaLegfAquarobotBody  double);  //  constructor  and  initializer 
"'AqusLegQ;  //  destructor 

double  GetLegAttaefameotAngleO  {  return  leg_attachment_angle;> 

int  GetMotionCompleteFlagO  {  return  motion_complete_flag;} 

void  SetLegAttacfameiitAnglefdouble  angle)  {leg^attachment_angle  »  angle;} 

void  SetMotionCoaq>leteFlag(int  flag)  {motion_coniplete_flag  ==  flag;} 

int  GetLegSuppoitFUgO  {  return  leg_support_flag;} 

void  SetLegSuppoctF1ag(iiit  flag)  {ieg_support_flag  ^  flag;) 

double  GetPleviousFootXCoordO  { return  previous_foot_x_coord;} 

void  SetPreviousFootXCoord(double  xcoord)  {inevious_foot_x_coord  =  xcoord;} 

double  GetPreviousFootYCoordO  { return  ptevious_foot_y_coord;) 

void  SetPteviousFootYCoord(double  ycoord)  {ptevious_foot_y  coord  =  ycoord;) 

double  GetPreviousFootZCoordO  {  return  ptevious_foot_z_coord;) 

void  SetPreviousFootZCoord(dooble  zcoord)  {previous_foot_z_coord  =  zcoord;} 

void  MoveIncremental(AquarobotBody  &,double  dettal,double  delta2, 

double  deita3); 

}; 

#endif 

//*«*«»•*•»«•***•***«•*«•**•••••••••*•••••*••***•***•* 

//  FILENAME:  AbRigid-C 

//  PURPOSE:  Implementation  of  class  RigidBody 

//CONTAINS: 

//**«,**.**«*****.***..*•***•.««*•**«•*•*»•****••*•••• 

#include  "AbRigidH” 

//  FUNCTION:  RigidBodyO 


RigidBody :  :RigidBodyO 

{ 

iiode_Iut  ~  new  ina&u(8,4,0.0); 
H  malrix  ~  new  iiiatrix(4,4,0.0X 


//  FUNCTION:  Update  Velocity 
//PURPOSE: 

//COMMENTS: 

n 

void  RigidBody;:UpdateVek>cit>(doiiUe  delta_t) 

{ 

u  »  u  +  (delu_t  •  udot); 

V  =  V  +  (delta_t  •  vdot); 
w  =  w  +  (deha_t  *  wdot); 
p  =  p  +  (delu_t  •  pdot); 
q  »  q  +  (delta_t  •  qdot); 
r  *  r  +  (delta_t  •  rdot); 

) 

//  FILENAME;  AbRigiAH 
//PURPOSE; 

// 

//COMMENTS: 

//•*••««««•**•••*•«••*••••••*«•«*••******•••••*••••*•*• 

#ifiridefH_RIGIDBODY 
#de&ie  H_RlGIDBODY 

iHinclude  <tiine.h> 

//#iDclude''Uiik.H'' 

^include  'MatrixMy.H' 

const  double  gravity  «=  32.218S; 


class  RigidBody 

{ 

private: 

//  double  location; 
double  ^y.v, 

//  velocityfd]; 
double  u,  v,  w,  p,  q,  r, 

//  acceleration[6]; 

double  udot,  vdot,  wdot,  pdot,  qdot,  rdot; 

//  fi)rces_and_torques(6]; 
double  Fx,  Fy,  Fz,  L,  M,  N; 

//  inoinents_of_inertia[3]; 
double  I]^  ly,  hi 
double  mass; 
long  cutTeflt_tiine; 

public: 

matrix  *iiode_list; 
matrix  *H_matrix; 

RigidBodyO; 

~RigidBodyO{};  //more  needed? 
void  TransfonnNodeListO; 

/• 

//  Use  these  for  dynamics::::::: 

matrix&  TranslateAndEulerAngleTratisform  (double  x,  double  y,  double  z. 


174 


double  azunuth, 
double  clevitiaa, 
double  roll); 
double  GelDelUT  0; 
void  StaitTimer  Q; 
void  UpdateRigidBody  0; 

•/ 


void  l^MlateAccelefatioB  Q; 

void  Update  Velocity  (double  delu_t); 

/* 

void  UpdateHMatrix  (double  delta_t); 
void  UpdatePocitiaa  0, 
void  CetNodePolygoaList  Q; 

•/ 


void  SetU(double  a)  {u  -  a;} 
void  SetV(double  a)  {v  =  a;} 
void  SetW(double  a)  (w  -  a;) 
void  S^P(double  a)  (p  -  a;} 
void  SetQ(double  a)  (q  =  a;} 
void  SetR(double  a)  (r  =  a;} 

double  GetUQ  {return  u;} 
double  GetVQ  {return  v,) 
double  CetWQ  {return  w,} 
double  GetPQ  {return  p;} 
double  GctQO  {return  q;} 
double  GetRO  {return  r,} 

void  SetUdot  (double  a)  {udot  =  a;} 
void  SetVdot  (double  a)  {vdot »  a;} 
void  SetWdot  (double  a)  (wdot  =  a;} 
void  SetPdot  (double  a)  {pdot  =  a;} 
void  SetQdot  (double  a)  {qdot »  a;) 
void  SetRdot  (double  a)  {r^  a;} 

double  GetUdotQ  {return  udot;} 
double  GetVdotO  {return  vdot;} 
double  GetWdotO  {return  wdot;} 
double  GetPdotO  {return  pdot;} 
double  Gt.<  JdotQ  {return  qdot;} 
double  GetRdotQ  (return  rdot;} 

void  SetFx(double  a)  {Fx  =  a;} 
void  SetFy(double  a)  {Fy  =  a;} 
void  SetFz(double  a)  {Fz  =  a;} 
void  SetL(double  a)  {L  =  a;} 
void  SetM(double  a)  {M  °  a;} 
void  SelN(double  a)  {N  ”=  a;} 

double  GetFxQ  (return  Fx;} 
double  GetFyO  {return  Fy,} 
double  GetFzQ  (return  Fz;} 
double  GetLO  {return  1;} 
double  GetMO  {return  M;} 
double  GetNQ  (retL  n  N;} 

void  Setlx(double  a)  (lx  =  a;} 
void  Setly(double  a)  {ly  =  a;} 
void  Setl^double  a)  {Iz  =  a;} 

double  GetlxQ  (return  Ix;} 
double  GetlyO  {return  ly,} 
double  GetlzO  (return  Iz;} 
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#cndif 


//  ••••••*««**«*M***M****]i]PS*k|J*PIQU*M*UM*MM«**«**M«****«U««** 

//rrrLE:  atcinLC 

//INPUT:  (x,y)body  coordinates  of*  selected  foot;  dircctioo;  body  cotter. 


//  ndius  of  wotlcspace;  CCW  arc  segnieot  endpoints;  CW  arc  segment 

//  em^wints;  foot  nunfoer  selected 

//OUTPUT:  intercepts  of  arc  segment;  distance  to  the  intercepts  fom  the 

//  current  foot  position;  flag  to  indicate  whether  an  irttercept 

//  was  found 

//FUNCTION:  This  fimetion  determines  whether  there  is  an  irttercept  of  the 
//  desired  direction  offoe  foot  widi  an  arc  segniditdefitimg  a  part 

//  ofthewotfomace. 

//**«*****«**«*«*«««*«*.^UAR0B0T**«*******«**********************< 


^include  <stdio.h> 

#iticlude  <mathJi> 

#definePI3.141S926d3» 

int  arciitt(double  foot[],  double  direction,  double  cenbod[],  double  radius,  double  neg_limit(],  double  pos_limit[],  irtt  foot_number, 
double  interceptsQ,  double  &diatance) 

{ 

//  initirUizatian 

double  a,  b,  c,  d,  x_itttercepl,  y_intcrcept; 

double  perp_distance,  len^  x_pc>P,  yjxtp; 

double  rad_s(|uared,  petp_squared; 

double  radius_test,  in_directioo,  out^directioo,  footrad; 

double  arc_foat,  neg_arc_liinit,  pos^arejimit,  limit  >  21S.0; 

int  arcsegnient_flag  >  0; 

// determine  if  foot  foils  on  the  arc  segment 
footrad  ”  sqfrt(  (foot{0]*foot{0]>+<foot[l]*foot{l])  y, 
if  (radius  =  footrad){ 

arcse0nertt_flag  >  1; 
ittteroepts[di  >  foot[0]; 
interoepts[l]  -foot(l]; 
distance  « limit; 

} 

if  (arcsegmettt_flag  =  0){ 

petp_distance  =  ((oenbod[4]  •  foot[l])*cos(direction)) 

-  ((oenbod(3]  -  foot[0])*sin(direction)); 


tf  (potp_distance  <=  radius){ 
radjtquared  =  radius*rsdius; 
perp_squared  =  perp_distance*perp_distance; 
if  (petp_distance  <  0){ 

lengfo  =  sqrt  (-(rad_squared  -  peTp_s<]uared»-, 

} 

else  { 

length  =  sqrt(rad_squared  -  perp  squared); 

} 

//  firxl  (x,y)  at  intersection  of  petpindicular  distance  and  length 
x_pop  =  cenbod[3]  +  (perp_distance*sin(direction)); 
y_perp  =  cenbod[4]  -  (petp_distance*cos(direction)); 


//  test  if  foot  is  inside  or  outside  radius 
a  =  (foot[l]  -  cenbod[4])*(foot[l]  -  cenbod[4]); 
b  =  (foot[0]  -  cenbod[3])*(fooit[0]  -  cenbod[3]); 
radius_test  =  sqrtfa  +  b); 

if  (radius_test  >  radius){ 

//  find  (x,y)  at  intersection  of  ray  and  arc  if  foot  is  outside  radius 
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x_atttoept  -  x_pap  -  (leiigtb*cai(<lirectioo)X 
y_mlcrc«pl  =  yjpeip  -  (leog|h*sin((b<ctiao)); 
intcrecpU(0]  -  x_iBlcrcc|]t; 
iiitaoq>tt[l]  =  y_aileree|it; 

} 

else  { 

//  (x,y)  intcnectioo  of  ny  and  arc  if  foot  is  intide  radius 

x_iiiic(cept  -  x_pctp  +  (Ieaglh*cos(directioa)); 
y_iiitcrce|it  >  y_pcn>  (lenglh*sin(directioa)); 
iiileroq)la[0]  -  x_iolerce|)t; 
intCTcepls[l]  -  y_ioiercepC; 

}  //  radius  test  cods  here 

// test  if  intersectioo  is  in  desired  directioa 

in_direction  =  foot{0]*cae(direction)  foot{l]*sin(direction); 

out_dir6CtioD  -  x_iiiiace|it*cos(directioo)  +  y_iidercept*sin(direction)-, 

if  (in_directioa  <*  out_difeGtioa){ 
c  ”  (y_intercept  -  fi)ot[l])*(y_!nteroept  -  foot(l]); 
d  =  (x_intercept  -  foot[0])*(x_inlercefit  -  foot[0]); 
distance  >  sqit(c  +  d); 


// test  if  intersection  is  within  arc  segment 

arcjbot  =  atan2(y_iEterce|it  -  cenbod[4],  x_intercept  -  cenbod(3)); 
neg_arc_liniit  =  atan2(neg^ltmit(l]  -  cenbod[4],  neg_liinit[0]  -  cenbod[3]); 
pas_arc_liniit  =•  stan2(pos_limit[l]  -  cenbod[4],  pos_timit[0]  -  cenbod[3]); 

// tests  to  ensure  proper  sections  of  arcs  are  used 
if  (foot_number  ==  1){ 

if  (arcJToot  >=  oeg^arc_li0iit  &&  arc_foot  <=  pos_arc_linut) 
arcsegment  flag=  1; 

} 

else  ( 

if(direction>0){ 
if  (neg^arc_Ui^  <  0) 
neg;_arc_liniit  =  neg;_arc_limit  +  (2*PI); 
if  (pos_arc_limit  <  0) 
pos_atc_limit  =  pos_arc_limit  +  (2*PI); 
if  (atc_foot  <  0) 

=  arc_foot  +  (2*PI); 

if  (arc_foot  >=  neg_arc_limit  &&  arc_foot  <=  pos_arc_Umit) 
aicsegmentflag  =  1; 

} 

else  {  //  directioa  <  0 
if  (neg_arc_limit  >  0) 
neg_arc_iimit  =  neg_are_limit  -  (2*PI); 
if  (pos_arc_limit  >  0) 
pos_arc_limit  =  pos_arc_lirait  -  (2*PI); 
if  (arc_foot  >  0) 
arc_foot  =  arc_foot  -  (2*PI); 

if  (aic_foot  >=  neg;_aic_liniit  &&  arc_lbot  <=  pos_arc_liniit) 
arcseginent_flag  =  1; 

}  //  direction  test  ends  here 
}  //£x>t  number  test  ends  here 
}// intersection  test  ends  here 
}  //  perpindicutar  test  ends  here 


if  (arcsegment_flag  !=  1){ 
atcsegineiit_flag  =  0; 

distance  =  limit; 
intercepts[0]  =  x_intercept; 
intercepts[l]  *  y_irtercept; 


}  //  nullifying  parameters  ends  here 
return  (atcsegmeiit_flag); 
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}  //  arant  fimctioa  endi  beie 


/»»—«»»«»««««»««««««««««»»»«»««»««»»»«»««>«»«»«»»«««»«»»««»««»««««»»»» 

FILENAME:  ucomLH 

PURPOSE:  Header  file.  This  file  leptcfcalt  AeluaroboCs  coaftaat  parameten 
COMMENT: 

#ifiidef_ARCONST_H 

#defiiie_ARCONST_H 

Mefine  PI  3. 141 5926S3589793 1 1 5997963468544183 161590576171875000 

#defineHPI(PI^.O) 

#<lefiiie  DR  (PI/180.0) 

Mefine  ROOT3  1.732050807368877193176604123436845839023590087890625 
Mefine  EQU_TR1ANGLE  3.0/2.0 

#defiiieL£G6 
#<lefiiieL£Gl  0 
#<iefineLEG2  1 
#defiDeLEG3  2 
#<lefiiieLEG4  3 
MefineLEG5  4 
MefineLEG6  5 

#4lefiiieCB0 
//define  HIP  1 
#define  KNEEl  2 
#defmeKNEE2  3 
#defineFOOT4 

#defineVJOINT4 
#defineJOINT3 
/WefineJOINTOO 
/WefineJOINTl  1 
/MefiiieJOrNT2  2 
/MefinelOINT3  3 
//define  JOINT4  4 

#define  LINKO  37.5 
#define  LINKl  20.0 
#defineLINK2  50.0 
#define  UNK3  100.0 

#define  LINK02  (37.5*37.5) 

#defuie  LrNK12  (20.0*20.0) 

//define  UNK22  (50.0*50.0) 

/Mefine  LINK32  (100.0*100.0) 

#derine  MAXMIN  2 


#define  TwoDm  2 
#derine  ThreeDim  3 
//define  XY  2 
#defineXYZ3 
#define  EULER  3 
//define  XW  0 
//define  YW  1 
/define  ZW  2 
#derine  XB  0 
//define  YB  1 
/MefineZB2 
/MefineXO 
//define  Y  1 
/MefineZ2 
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Mefine  RcsetThctel  (0.0) 
Mefine  RaetTbete2  (66.4) 
Me&BC  ResctThele3  (-136.4) 

Mt&ut  StaitThetaOLegl  0.0 
#define  SUftTbetaOLe^  60.0 
#<]efine  Sta(tTheU0Leg3  120.0 
#<lefiiie  Stait'nieU0Le84  180.0 
#defiiie  StartTheUOLegS  240.0 
^define  SUftT1)eU0Leg6  300.0 

Mefine  StaitTheUl  (0.0) 

#defiiie  StaitTheU2  (33.86) 
#defiiie  SuitTheU3  (-123.86) 

MeGstt  TbetelNegLim  (-60.0) 
Mefine  Thetc2NegLiin  (-106.6) 
Mefine  Thete3NegLim  (-136.4) 

^define  ThetelPosLim  (60.0) 
#define  Thete2PosLim  (73.4) 
MeBae  Thete3PosLiin  (23.6) 

#define  StaitX  98.02 
#define  StaitY  0.0 
#defiiie  StaitZ  0.0 

#define  TripodSize  98.02 
UdeEae  StartTripodSize  98.02 
^define  DefauttSTRIDE  120.0 
#defiiie  DefaultFOOTheight  13.0 

/* 

#defiiK  HighHight  120.0 
#<lefuie  MtidiumHigbt  100.0 
#defiiie  LowHight  80.0 

•/ 

#deiine  InitialElpsRatio  2.0 

^define  JlUmN  (-60.0*DR) 
#defiiie  JlLimP  ( 60.0«DR) 
#de£ine  J2LimN  (-106.6*DR) 
#defiiieJ2LimP(  73.4»DR) 
#define  J3UmN  (-156.4»DR) 
#defineJ3UmP(  23.6*DR) 

//^define  ORIENTATION  3 
#deiine  AZIMUTH  0 
^define  ELEVATION  1 
Udefme  ROLL  2 

Adeline  YAW  0 
#define  PITCH  1 
#defmeROLL2 

/• 

^define  AZIMUTH  3 
#define  ELEVATION  4 
#defme  ROLL  3 

#defme  YAW  3 
#definePITCH4 
#define  ROLL  3 
*1 


#ifiidef  arfi;nc_c 

#define_ARFUNC_C 
//  FILENAME:  ufunc-C 

//  PURPOSE:  Basic  Mathematical  Functions  for  Aquarobot  Simulation. 

ttinclude  <stdio.h> 

#include  <inath.h> 

#include  ’arconstH" 

#include  'Kinematics-H" 

//TITLE:  minO 

//  FUNCTION:  minimum  operation 
//INPUT: 

//OUTPUT: 

//RETURN: 

//COMMENT: 

//DATE: 

double  min(doubIe  x,  double  y) 

{ 

il(x<y) 
return  x; 
else 

return  y, 

} 

//  TITLE:  maxO 
//  FUNCTION:  maximiun  operation 

double  max(double  x,  double  y) 

{ 

il(x>y) 
return  x; 
else 

return  y, 

} 

//  TITLE:  ellipseO 

//  FUNCTION:  Calculates  incremental  foot  point  along  an  elliptical  path 
//  generated  between  the  last  step  and  the  next  step. 

//INPUT: 

//OUTPUT: 

//  CALLED  BY:  tripodOphaseQ.  tripodlphaseO,  bodyphaseQ. 
//COMMENT:  Tte  function  is  for  continuous  motion. 

//  This  function  divides  ellipse  angle  into  FINE. 


void  ellipsefWallcParameter  &wp,  //  walking  parameters 

double  footprintQ,  // 
double  footpointQ)  // 

{ 

double  theta;  //  ellipse  angle 
double  d_theta;  //  dumge  in  ellipse  angle 
//  double  d_segnien^  //  ellipse  segment  speed 
double  a,  b;  //  x_diameter,  y_diameter 
double  x,  y,  z;  //  ellipse  coordinates 


a  =  wp.stride  /  2.0; 
b  =  wp.footheight; 

/•  Coordinates  Transformation(WORLD->ELLIFSE)  */ 
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X  -  cot(wp.diiectioa*DR)*(footpoinipCW]  -  footprintpCW]) 

+  sin(wp.(lirectioo*DR')*(footpoiiit[YW]  -  fixNpnntfVW]); 
y  =-  (foo4)omt[ZW]  -  foot|iirint(ZW]); 
z  =-  siii(wp.<iiicctioo*DR)*(fiM<poiiitixW]  -  footpriot(XW]) 

+  ca«(>wp.(it(ectioii*DR)*(foatp^[YW]  -  foolprintfYW]); 

/*  to  olculite  cumnt  ellipie  angle  */ 
theta  »  atan2((y/b),  (x-aya); 

d_theU  *  PI/FINE; 

/*  decrement  ellipse  angle  */ 
theta  —  d_theta; 


/*  to  calculate  next  foot  point  */ 

X  =  a*(1.0  +  cos(theta)); 
y  =•  b*sin(theta); 
z  =  0.0; 

if(theta<=0.0){ 

theU=°  180.0; 

} 

/*  to  calculate  foot  position  on  World  coordinates  */ 

footpointpfW]  =  x*oos(wp.directioo*DR)  -  z*sin(wp.directioa)  +  footprintpOV]; 
footpointfYW]  »  x*sin(vvp.direction*DR)  +  z’co^wp.direction)  +  footprintfYW]; 
footpoint(ZW]  =  -  y  +  foolprintlZWj; 


} 

//  TITLE:  kinematicsQ 

//  FUNCTION:  Computes  Kinematics  for  .\quarobot  Type  II. 

//  INPUT :  Joint  An^es(1heta[CB->FOOT]  Unit:degree). 

//  OUTPUT:  Joint  Positions  related  to  BODY  coordinates(Unitcm). 

H  CALLED  BY :mv_kinematicsO,  tripodOphaseO;  tripodlphaseO.  bod>phaseO 

void  kioematics(double  thetaQ,  //  Joint  Anglt 

double  hip[],  // HIP  Joint  Position(BODY) 
double  kneelQ,  //  KNEEl  Joint  Position(60DY) 
double  knee2[],  //  KNEE2  Joint  Pasition(BODY^ 
double  footQ)  //  FOOT  Joint  Positioo(BODY) 

{ 

double  cO,  sO,  cl,  $1,  c2,  s2,  c3,  s3; 
double  cOl,  sOl,  c23,  s23; 
inti; 


foif  i=0;  i<4;  i++){ 
th^[i]  ==  theU[i]*DR; 

} 

cO  =  cos(tlieta[0]);  sO  =  sin(theta[0]); 

cl  =  co$(theta[l]);  si  =  sin(thela(l]); 

c2  =  cos(theta[2]y  s2  =  sin(theta[2]); 

c3  =  cos(thetaP]);  s3  =  sin(theta[3]); 

cOl  =  co^theta[0j-Hbeta[l]);  sOl  =  sin(theta(0]-»'theta[l)); 

c23  =  cas(theta[2]-Hheta[3]);  s23  =  sin(theta{2]+dieta[3]); 

hippCB]  “  LINK0*c0;  //  HIP  Position 
hip[YB]  =  LINK0*s0; 
hip[ZB]  >  0.0; 

kneeipffi]  -  hippCB]  +  UNKl’cOl;  //  KNEEl  Position 
lcneel(YB]  =hip(yB]  +  LlNKl*s01; 
kneel[ZB]  =  0.0; 

knee2pCB]  =  kneelfXB]  +  LINK2*c01*c2;  //  KNEE2  Position 


182 


kiiee2[YB] '  kneel[YB]  +  LINK2*s01»c2; 
kfiee2[ZB]  ^UNK2*s2; 

footpffl]  =  taiee2[XB]  +  LINK3*c01»c23;  //  FOOT  Positioo 
foot(YB]  =  kiiee2[YBJ  +  LINK3**01*c23; 
foot(ZB]  -  k]iee2[ZB]  -  LINK3*s23; 


i<4;  i++){ 

tb^i]  >  theU(i]/(DR); 

> 

} 


//  TITLE:  inv_kiiiematicsO 

//  FUNCTION:  Computes  Inverse  Kinematics  for  Aquarobot  Type  II. 

//  INPUT:  FOOT  position  in  World  Coon]inates(footpCYZ]). 

//  OUTPUT:  Joint  Angles(Unitdegree). 

//  CALLED  BY :tiipodOpliaseO,  tripodlphaseQ,  bodyphaseQ 
//  CALLS:  kinem^csO 

void  inv_kinematics(double  footQ,  //  FOOT  Position(BODY) 

double  tbetanV/  Joint  Angle 

{ 

double  px,  py,  pz; 

double  px2.py2,pz2;  /•  px^,  P!^  pz^  •/ 

double  xO,  yO,  zO;  /*  CB  coonlinates  origin  */ 

double  cO,  sO,  cl,  si;  /*  sin(theta_i),  cos(tbetaJ)  */ 

double  c3,  s3;  /*  sin(theta_i),  cos(thetaJ)  */ 

double  cOl,  sOl;  /*  si^theta_i+theta JX  cos(thte_i-HlietaJ)  */ 

double  bl.b2.b3;  /*  length  bi*/ 

double  bl2.b22,b32;  /*bi'^*/ 

double  beta;  /*  angle  beta  */ 

double  cpsi,  spsi;  /*  cos(psi),  sin(psi)  */ 

double  theta2j),  theta2_n; 

double  theta3_p,  theta3_n; 

double  fla^ 

double  4(4];  !*  joint  angle  •/ 

double  l9[3],  lcl[3],  k2[3],  ^3]-,/*  joint  position  *! 
inti; 

px  =  footjO];  px2  =  px*px; 
py  «  foot[l];  py2  =  py*py, 
pz  =»  foot[2];  pz2  =  pz*pz; 

theU[0]=tbeU(0]*DR; 
cO  =  c^tiieta(0]); 
sO  =  sin(tlieta[0]); 

bl2  =  (px-LINKO»cO)*(px-LINKO*cO)  +  (py-LINKO’sO)*(py-UNKO»sO); 

bl  =s(pt(bl2); 

b2  =bl-LINKl; 

b22  -  b2*b2; 

b32  =  b22+pz2; 

b3  =s<jrt(b32); 

/•theUl  •/ 

cl  =  (  px2+py2-LINK02-bl2  y(  2.0»LINK0*bl  ); 
cl  =niiii(1.0,  cl); 
si  "■sqi1(1.0-cl*cl); 

/*  position  invetse  transformation  from  CB  to  JO  coordinates  */ 
xO  =  px*cO  +  py*sO; 
yO  =-px*sO  +  py*cO; 
zO=pz; 


ifr:x0<=0.0 ){ 
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ptintfC#####  XB  range  ennr  #####«'); 

> 

ifl;  yO  >“  0.0  ){ 
thi^l]  atiui2(  si,  cl); 

} 

ebe{ 

tb^l]  =  aUn2(-«l,  clX 

> 

if(  (theU[l]  <  JlLtmN)  &&  (theU{l]  >  JlLimP)  ){ 
printfT#####  NO  SOLUTION  FOR  THETAl  (WHWWftn*); 

} 

cOl  =>  cas(theU[0]-Kheta[l]); 
sOl  =  $in(tbeta{0]-i-OieU[l]); 
theU(0]>theU[0]/(DR); 
theU[l]=°theta{l]/(DR); 

/•theta2*/ 

beta  atan2(  pz,  b2 ); 

cpsi  =  inin(1.0.  (  UNK22+b32-LINK32  )/( 2.0*LINK2‘b3  )); 
^  -  s<pt(1.0-cpsi*cpst); 


theta2_p  =  ( atan2(  q)si,  cpsi)  -  beta)/(DR); 
tbeta2_n  >=  (  atai)2(-^ist,  cpsi)  -  beU)/(DR); 

/•theta3»/ 

c3  =  iniii(1.0,  (  b32-LINK22-LINK32  )/( 2.0*UNK2‘LINK3  )); 
s3  =  sqrt<1.0-c3*c3); 
theta3_p  aUn2(  s3,  c3)/(DR); 

1heta3_n  =  atan2(>s3,  c3)/(DR); 

/*  selection  ofproper  combination  of  ttieta2  and  theta3  */ 

th(0]==tlieU[0];  th[l]=theta[l]; 

flag^; 

fot(i=0;  i<4',  i++){ 
switcfa(i){ 
caseO: 

th(2]  =  theta2j>;  th(3]  =  theta3_ii; 
break; 
case  1; 

th[2]  =theta2  n;  th[3]  =theta3_n; 
break; 
case  2: 

th(2]  *lbeta2_n;  th[3]  =  theta3_p; 
break; 
case  3: 

th[2]  =  theta2_p;  th[3]  =  theta3_p; 
break; 

} 

kinematics(th,  bp,  kl,  k2,  ft); 
iff  (fabs(px-ft[XB])<1.0e-3) 

&&  (fabs(py-ft{YB])  <  l.Oe-3) 

&&  (fabs(pz-ft(ZB])  <  l.Oe-3) ){ 
theU[2]  =th[2]; 

1hela[3]  =1h[3i; 

flag=l; 

break; 

} 

} 

ift;flag=0) 

printfi:"#####  NO  SOLUTION  FOR  THETA2  &  THETA3  #####  \n"); 


//*•«****•*****•*•*•»»»**•*•****.**•*••«*.**«*.•.**.•.•.•*>« 

//  TITLE:  world_bodyO 

//  FUNCTION:  Cooidinate  transfoimation  from  BODY  to  WORLD 
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//INPUTS;  euleraiiglci(eulcr(p(i,theU^]XOngioafBOOYcoor<lioaies. 

//  A2iinulh,ElevaliotUIoU 

//  Yaw,Pildi.R<^ 

//OUTPUTS;  WORLD ooofdinata 

//  CALLED  BY  iripodOphaseQ,  tripodlphascO,  bodyphaaeO 

void  wcirld_bo<iy(double  culcrQ,  // Euler  angles 

double  orgQ,  //  Origin  of  BODY  coordinales  on  WORLD 
double  positiooQy/  cootxiinates 

{ 

double  cl,  c2.  c3,  si,  s2,  s3; 
double  X,  y,  z; 

cl  ~  oosfeuletf  AZIMUTH]);  sl=°sin(euler[AZIMUTH)); 
c2  =  co8(euler(ELEVATION]);  s2=sin(euler{ELEVATIONl); 
c3  =  co6(euler[ROLL]);  s3^=sin(euler[ROLL]); 


X  »  positiaapCB]; 
y  =  positionfYB]; 
z  =  positioofzB]; 


poshionpCW] «  cl*c2»x  +  (cl*s2‘s3.sl»c3)»y  +  (cl«s2*c3+sl*s3)‘z  +  org[X]. 
positknnrW]  =  sl»c2*x  +  (sl»s2»s3+cl»c3)»y  +  (sl*s2»c3-cl»s3)*z  +  ofg(Y]; 
position[ZW]  =  -s2*x  +  c2*s3*y  +  c2*c3*z  +  org(Z]; 

} 

/'TITLE:  body_worldO 

//  FUNCTION:  Coordinate  transformation  from  WORLD  to  BODY 
//  INPUTS:  euler  aogles(euler[psi,thetaj)lii]).  Origin  of  BODY  coordinates. 

//  Azimuth,Elevatiofi,Rotl 

//  Yaw,Piteh,RoU 

//OUTPUTS:  BODY  coordinates 

//  CALLED  BYiripodOphaseO,  tiipodlphaseO,  bodypiuseO 

//t**.****.******...***.***.....**.**....*.*.*..*.**.*..*..***..*.*....* 

void  body_world(double  eulerQ,  //  euler  angle 

double  orgl),  //  origin  of  BODY  coordinates  on  WORLD 
double  poshionQ)  //  coordinates 

{ 

double  cl,  c2,  (3,  si,  s2,  s3;  /*  cosQ,  sinQ  */ 
double  X,  y,  r,  /*  WORLD  coordinates  */ 


cl>cas(euler{AZIMUTH]);  sl=sin(euler[AZIMUTH]); 
c2=cos(euler(ELEVATION]);  s2=sin(euler[ELEVATION]); 
c3>«Qs(euler(ROLL]);  s3=sin(euI«piOLL]); 


X  “  positionpCW]; 
y  =  positioo[YW]; 
z  =  position[ZW]; 


positioopCB]  =  cl*c2*(x-otgpCW])  +  sl*c2*(y-oigPfW])  -  s2*(z-oig{ZW)); 
poshionfYB)  =  (cl*s2»s3-sl*c3)*(x-orgpCW]) 

+<sl»s2*s3+cl»c3)*(y-orgrYW])  +  c2*s3»(z-orgtZW)); 
posiUofi[ZB]  =  (cl‘s2»c3+sl»s3)*(x-orgpCW]) 

+(sl*s2*c3<l*s3)*(y-oiglYW])  +  c2*c3»(z-orgtZW]); 

> 

#endif 

//TITLE:  maxdistaiice_25cmfoot0 

//INPUT:  (x,y)body  coordinales  ofall  six  feet  and  body  center,  tripod 
//  numbCT(0  =  tiipod0, 2=  tripod  1);  direction 

//OUTPUT:  maxtoHun  stride  possible  without  exceeding  wof1(q>ace 
//FUNCTION:  This  function  finds  the  maximum  stride  possible  for  AquaRobot, 
//  using  a  tripod  gait,  given  an  arbitrary  dir^on  as  an  input 

//***.*..*..*.*«...*.«.*y^QUAROBOT****************************»** 
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double  maxdisuiice  2Sciiifool(Nc]a_Matioa  ten.  WaOcPenaieter  Awp) 

{ 


int  «rcml(double*,  double,  double*,  double,  double*,  double*,  im,  double*,  douUe  ft); 
int  aegiin(double*,  double,  double*,  double*,  double*,  double  ft>, 

//  initulizaliaa 

double  iond 37.3;  // imer  radius  of  woik^Moe 

double  outnd-  178.2107;  //outer  radius  of  workspsce 

double  msxdist.  distsnce_s,  disUiics_b,  disUacc_c,  limit  ~  36.31; 

double  mtetctpts{2]; 

double  mdistsiioe  ~  0.0; 

int  interccpt_flag  ==  0; 

int  footnum,  convert; 

double  body_oenler{3]; 

double  cul^3]; 

double  sfoot(3]; 
double  bfi>at[3]; 
double  cfoot(3]; 
double  dfoo([3]; 
double  efoot(3]; 
double  9bot{3]; 
double  oenbod(6]; 

for  (convert  =  0;  convert  <*  2;  convert-*-*-)  { 

afbotfeonvert]  -  nmi(>ot_l_coarid[convert]; 
bfoo([oonvert]  -  nmibot_2_coord[convert]; 
cfbot[oonvert]  =•  nnifoot_3_coord(coovert  j; 
dfooC(oonvert]  •  anLfoot_4_coord{convert]; 
efoo((caavert]  =  nnLfoot_3_coord(coovertj; 
ffoot(ooiivert]  «  nm-foot  6  ooord(oonvert]; 

} 

for  (convert  »  0;  convert  <•  3;  oonvert-*-*-)  { 

oenbod[convert]  •  nm-body  center  ooord(ooovert]; 

} 


double  dir  ~  wp.direction  *  DR; 
iid  tripod  =  vrp.phase; 


for(coovert  =  0;  convert  2;  convert-*-*-)  { 

euIer(coovert]  =  nm.body_center_ooocd(convert]; 
body_center[cQnvert]  =  nm.body_oenter  coord(convert-*-3]; 

} 

ststic  double  seglpin[2]  =  {36.8686, 6.8324}; 

// inside  endpoint  of  CW  segment  #1 

ststic  double  seglpout(2]  =  {160.2049, 78.0606}; 

//  outside  endpoint  of  CW  segment  #1 
static  double  segi:iin[2]  °  {36.8686,  >6.8324}; 

//  inside  endpoint  of  CCW  segment  #1 

ststic  double  segliiout(2]  { 160.2049,  -78.0606}; 

//  outside  endpoint  of  CCW  segment  #1 

static  double  seg2pin[2]  >  {12.3, 33.3333}; 

//  inside  endpoint  (^CW  segment  #2 
static  double  seg2pout{2]  ^  {12.3, 177.7718}; 

// outside  endpoint  of  CW  segment  #2 
static  double  seg2nin[2]  ^  {24.3686, 28.3030}; 

//  inside  endpoint  of  CCW  segment  #2 

static  double  seg2nout[2]  =  {147.7049, 99.7112}; 

// outside  endpoiin  of  (XW  segment  #2 

static  double  seg3pin[2]  >  {-24.3686, 28.3030}; 

// inside  endpoint  of  CW  segment  #3 

static  double  seg3poot(2]  =  {-147.7049, 99.71 12}; 

//  outside  endpoint  of  CW  segment  #3 
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static  doubUseg3mii[2]-  (-12.5, 3S.3SS3}; 

// inside  codpoiiit  of  CCW  sc^nent  #3 
static  double  seg3noiJt{2]  •  {>12.3. 177.7718}^ 

// outside  endpoint  of  CCW  sepnot  #3 

static  double  seg4pin(2]  >  {-36.8«86,  -6.8324}; 

//inside  endpoint  of  CW  segment  #4 

static  double  seg4pout[2]  -  {-160.2049,  -78.0606}; 

// outside  endpoint  of  CW  segment  #4 

static  double  seg4iun(2I  (-36.8686, 6.8324}; 

//  inside  endpoint  of  CCW  segment  #4 

static  double  seg4iiout{2]  =  (-160.2049, 78.0606}; 

//  outside  endpoint  of  CCW  segment  #4 


static  double  seg3pin[2]  =  (-12.3,  -33.3333}; 

// inside  mdpoim  of  CW  segment  #3 

static  double  seg3pout[2]  =  (-12.3,  -177.7718}; 

//  outside  endpoint  of  CW  segment  #3 

static  double  seg3nin[2] »  (-24.3686,  -28.3030}; 

// inade  endpoint  of  CCW  segment  #3 

static  double  seg3nout[2]  ^  (-147.7049,  -99.7112}; 

//  outside  endpoint  of  CCW  segment  #3 

static  double  seg6pin[2]  >  (24.3686,  -28.3030}; 

//  inside  endpoint  of  CW  segment  #6 
static  double  seg6pout[2]  =  ( 147.7049,  -99.71 12}; 
//  outside  endpoint  of  CW  segment  #6 
static  double  scg6nin(2]  =  { 12.3,  -33.3333}; 

//  inside  endpoint  of  CCW  segnient  #6 
static  double  seg6oout[2]  =  (12.3,  -177.7718}; 

//  outside  endpoint  of  CCW  segment  #6 

//  convert  from  WORLD  to  BODY  ooofdinates 
body_world(euler,  body_center,  afoot); 
body_world(euler,  body_center,  bfoot); 
body_worid(euler,  body_cen(er,  cfbot); 
body_worid(euler,  body_oenter,  dfoot); 
body_\vorid(euler,  body_center,  efoot); 
body_worid(euler,  bod>_center,  ffoot); 
body_worid(euler,  body_center,  &oenbod[3]); 

if  (tripod  =  0){ 


//  test  footl  in  leg  1  woricspace 

foocnum  =  1;  //  select  foot  number  one 

//  test  for  intersection  with  outer  workspace  limit 

intercept_flag  »  arcintfafoot,  dir,  cenbod,  outrad,  seglnout,  seglpout,  footnum,  intercepts,  mdistance); 
if  (intercept_flag  =  1) 
distance_a  =>  mdistance; 


//  test  for  intersection  with  inner  workspace  limit 

intercept  flag  =  arcintfafoot,  dir,  cenbod,  inrad,  seglnin,  seglpin,  footnum,  intercepts,  mdistance); 
if  (mtercept_flag  =  1) 
disunce_a  =  mdistance; 


//  test  for  intersection  with  CW  workspace  limit 

interceptflag  =  segint(afoot,  dir,  seglpin,  seglpout,  intercepts,  mdistance); 
if  (intercept_flag  =  1) 
distance_a  mdistance; 

//test  for  intersection  with  CCW  workspace  limit 
intercept_flag  =  segint(afoot,  dir,  seglnin,  seglnout,  intercepts,  mdistance); 
if  (intetcept_flag  ==  1) 
distance_a  =  mdistaiKe; 


//  test  foot3  in  leg  3  workspace 


187 


fgotDum  «  3;  //  select  foot  Dumber  three 

// test  for  inlersectiaa  with  outer  workspace  limit 

inleroept_flag  -  arcinl(cfoat,  dir,  oeriwd,  outnd, 

segSnout,  segSpout,  footoum.  inletcepls,  mdistance); 

if  (inlsroept_flag  "1) 
distaiioe_b  -  mdistaiim; 

// test  for  intenectko  with  bmer  workspace  limit 
inlsrcept_flag  -  afciiit(cfoot,  dir,  oenb^  inrad, 

teg3nin,  segSpin,  footnum,  intercqDs,  mdistance); 

if  (inleroqit_flag  “1) 
distaiice_b  mdistaiice; 

//  test  for  intefsection  with  CW  workspace  limit 
intercept_flag  =  segintfcfoot,  dir,  segSpin,  segSpout, 

intercepts,  mdistanceX 

if  (interoept_fUg  “1) 
distance_b  =  mdistance; 

//  test  for  intetseclion  with  CCW  workspace  limit 
imerccpt^flag  >=  tegifit(cfoot,  dir,  segSnin,  segBnout, 

intercepts,  mdistance); 

if(interoept_flag  —  1) 
distance_b  mdistance; 

//  test  foot5  in  leg  5  workspace 
footnum  =  3;  //  select  foot  number  five 
//  test  for  intersection  with  outer  workspace  limit 
iot<rcept_flag  °  arcintfefool,  dir,  cenb^  outrad, 

seg5nout,  segSpout,  footnum,  intercepts,  mdistance); 

if  (inlercept_flag  “  1) 
distance_c  ~  mdistance; 

//  test  for  intersection  with  inner  workspace  limit 
intercept_flag  arcint(efoot,  dir,  cenbod,  inrad, 

seg5iun,  seg5pin,  footnum,  intavefMs,  mdistuKt); 

if  (intercept_flag  "■  1) 
distance_c  ~  mdistance; 

//  test  for  intersection  with  CW  workspace  limit 
intercept_ilag  tegint(efoot,  dir,  segSpin,  segSpout, 

intercepts,  mdistance); 

if  (iiitercept_flag  “  1) 
dislance_c  -  mdistance; 

//  test  for  intersection  with  CCW  workspace  limit 
intetcept_flag  ^  seguitfetboi,  dir,  segSnin,  segSoout, 

intercepts,  mdistance); 

if  (ir*ercept_flag  ”  1) 
distance_c  -  mdistaiKe; 


maxdist  =  min(min(distance_a,  distanoe_bX  distance_c); 


}  //this  ends  tripod  0  testing 

else  if  (tripod  “  2){ 

//  test  foot2  in  leg  2  workspace 

firotnum  -  2;  //  select  foot  number  two 

//  test  for  intersection  with  outer  workspace  limit 

iiitercept_flag  •  arcint(bfool,  dir,  cenbod,  outrad, 

seg2nout,  seg2pout,  footnum,  intercepts,  mdistance); 

if(intereept_flag“  1) 
distance_a  ~  mdistaiKe; 

// test  for  intersection  with  inner  workspace  limit 
intercept_f1ag  ~  arcint(bfoot,  dir,  cenbod,  inrail. 


188 


ttfflpia,  footnum,  iaterccpti,  mdistaoce); 

if(inlei«ep«_fl«g—  1) 
diiUiice_a  -  md»»«iice; 

// tcft  for  ataiectioa  with  CW  woricqMce  Umit 
intereept.fUg  >  tcgiolfhfool,  dir,  se^pm.  scg2pout, 

intercepts,  mdisunoe); 

if  (inlercepl_flsg  *=1) 
disUiice_s  ~  mdistsoce; 

// test  for  iidenectioo  with  CCW  woriespux  limit 
inlercept_flag  >  seginl(bfoot,  dir,  legZnin,  segZnout, 

intercepts,  mdistsoce); 

if  (intetcept_fUg  “  1) 
distance_s  >  mdistsoce; 

// test  foot4  in  leg  4  wotkspsce 

footnum  ~  4;  //  select  foot  number  four 

// test  for  intenection  with  outer  wotkspsce  limit 

interGept_flsg  =  sicint(dfoot,  dir,  cenb^  outrsd, 

seg4nout,  seg4pout,  footnum.  inlercepts,  mdistsoce); 

if  (iidercept_flsg  "  1) 
distsnce_b  °  mdistsoce; 

// test  for  intersection  with  inner  wotkspsce  limit 
intercept_fUg  =  srcint(dfoot,  dir,  cenb^  innd, 

seg4nin,  seg4pin,  footnum,  intercepts,  mdistsoce); 

if(intercept_flsg-=-  1) 
distsoce_b  -  mdistsoce; 

//  test  for  imeisection  with  CW  wotkspsce  limit 
intercept.flsg  »  segint(dfoot,  dir,  seg4pin,  seg4poiit, 

intercepts,  mdistsoce); 

if  (intercept_flsg  =  1) 
distsDce_b  •  mdistsnce; 

// test  for  intersection  with  CCW  wotkspsce  limit 
imeicept_flsg  =  segiiitfdfoot,  dir,  seg4nin,  seg4oout, 

intercepts,  mdistsnce); 

if  (intercept_llsg  =  1) 
distsnce_b  =  mdistsnce; 

//  test  foot6  in  leg  6  wotkspsce 
footnum  =  6;  //  select  foot  number  six 
// test  for  intersection  with  outer  work^Mce  limit 
iiiterccpt_fUg  =  sicint(ffoot,  dir,  cenbod,  outrsd,  segdnout,  seg6pout, 

footnum,  intercepts,  mdistsnce); 

if  (intetcept_flag  =  1) 
distance_c  =*  mdistsooe; 

// test  fm- inteisection  with  inner  wotkspsce  limit 
intercept_flag  =  arcintfffoot,  dir,  cenbod,  inrad,  seg6nin,  seg6pin, 

footnum,  intercepts,  mdistsnce); 

if  (intercept_flsg  =1) 
distance_c  =  mdistance; 

//  test  for  inteisection  with  CW  workspace  limit 
intereept_ilag  =  segint(£fool,  dir,  seg6pin,  seg6pout, 

intercepts,  mdistance); 

if  (intercept_flag  =1) 
distance_c  =  mdistance; 

//  test  for  intersection  with  CCW  workplace  limit 
iiitercept_flag  =  segintfffoot,  dir,  s^nin,  segfinout, 

intercepts,  mdistance); 

if(intercept_flag==  1) 
distance_c  =  mdistance; 


maxdut  >  min(niiii(dMliiice_a.  disUBce_b),  distaoce_cX 
}  //  tUt  OKb  tripod  1  toriiiig 
return  (imxdiit); 

}  //  thit  ends  fimction  ouxdiftaiice_2}cDifoat 

//*«*M**..****««M*****«*>n>S*aod«PHIU*****************************' 

//TITLE:  inixilwt«nce_45cinfoot0 

/'L'lPUT:  (x,y)body  coordinates  ofallrix  Ext  and  body  center,  tripod 
''  munber  (0  -  tripod  0, 2  -  tripod  1);  directico 

//OUTPUT:  nuudmum  stride  possible  without  exceeding  workspace 
//FUNCTION:  This  function  finds  the  maximuin  stride  possible  for  AquaRobot, 


//  using  a  tripod  gait,  given  an  arbitrary  dire^on  as  an  mput 

//..*«....*«****«««***..AQI;AR0B0T«************************< 


double  nuxrtistance  43ctnfool(Ne)d_Motion  ftam,  WaDcParameter  Ikwp) 

{ 


ini  arcintfdouble*.  double,  double*,  double,  double*,  double*,  mt,  double*,  double  A); 
iitt  segiiil(double*,  double,  double*,  double*,  double*,  double  A); 

//initialization 

double  inrad  •  43.0;  //  imier  radius  of  workspace 

double  outrad  •  149.27;  //  outer  radius  of  workspace 

double  maxdist,  distaiKe_a,  distanoe_b,  distance_c; 

double  inlerccpts[2]; 

double  diitatKy  =  0; 

int  itnetcept_iUg  ~  0; 

ini  footmim; 


double  afoot[3]; 

double  bfool[3]; 

double  cfoot(3I; 

double  dfoot[3]; 

double  efoot(3]; 

double  ffoot[3]; 

double  cenbod(fi]; 

afoot[3]  =  nm.foot_l_coord(3]; 

bfool(3]  =  niafoot_2_coord[3]; 

cfoot[3]  =  niafoot_3_eoord[3]; 

dfoot(3]  =  rBn.foot_4_ooord[31; 

efoot(3]  =  nnLfoot_3_coord(3]; 

fibot[3]  =  nitLfoot_6_coord(3]; 

cenbod[6]  =  nm.body_center_coord(6]; 

double  dir  =  wp.direction; 

mt  tnpod  =  wp.phase; 

static  double  seglpin[2]  =  {43.0,  0.0}; 

//  inside  endpoint  of  CW  segment  #1 
stauc  double  seglpout[2]  =  {139.0446,  34.2967); 
//  outside  endpoint  of  CW  segment  #1 
static  double  seglnin[2]  =  {43.0, 0.0); 

//  inside  endpoint  of  CCW  segment  #l 

static  double  seglitout{2]  »=  {139.0446,  -34.2967}; 

//  outside  endpoint  of  CCW  segment  #1 

static  double  seg2pin(2]  =  {22.3, 38.971 1 }; 

//  inside  endpoint  of  CW  segment  #2 
static  double  seg^pout[2]  =  {22.3, 147.3643}; 

//  outside  endpoint  of  CW  segment  #2 
static  double  seg2nin[2]  =  {22.3, 38.971 1 }; 

//  inside  endpoint  of  CCW  segment  #2 

static  double  seg2iKiut[2]  -  { 1 16.3446, 93.2678}; 

//  outside  end^int  of  CCW  segmeid  #2 
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sUtic double scg3pii>(2]  ^  {-22.3, 38.9711}; 

//  inside  endpoint  of  CW  segment  #3 
static  double  seg3pout{2]  =  {-1 16.5446, 93.2678}; 
//  outside  endpoint  of  CW  segment  #3 
static  double  scg3nin[2]  =  {-22.5, 38.971 1 }; 

//  inside  endpoint  of  CCW  segment  #3 
static  double  seg3nout[2]  ~  {-22.3, 147.3643}; 

//  outside  endpoint  of  CCW  sepnent  #3 


static  double  seg4pin(2]  {-45.0, 0.0}; 

//  inside  endpoint  of  CW  se^nent  #4 

static  double  se^pout[2]  =  {-139.0446,  -54.2967}; 

//  outside  end^toint  of  CW  segmem  44 
static  double  seg4nin(2]  =  {-43.0, 0.0}; 

//  inside  endpoiid  of  CCW  segment  #4 

static  double  seg4nout(2]  =  (-139.0446,  54.2967); 

//outside  endpoint  of  CCW  se0nent#4 

static  double  seg3pin[2]  =  {-22.5,  -38.971 1 }; 

//  inside  endpoint  ofCW  segment  #3 

static  double  seg3pout[2]  °  {-22.3,  -147.3645}; 

//  outside  endpoint  of  CW  segment  #3 
static  double  seg5oin(2]  >  (-22.3,  -38.971 1 }; 

//  inside  endpoint  of  CCW  segment  #3 

static  double  seg5nout[2)  =  {-116.5446,-93.2678); 

//  outside  endpoint  of  CCW  segment  43 

static  double  seg6pin[2]  =  {22.3,  -38.971 1 }; 

//  inside  enc^int  of  CW  segment  46 

static  double  seg6pout(2]  =  {1 16.5446,  -93.2678); 

//  outside  endpoint  of  CW  segment  46 
static  double  seg6oin[2]  =  {22.5,  -38.9711 }; 

//  inside  endpoint  of  ^W  segment  46 
static  double  seg6nout{2]  =  {22.3,  -147.3643); 

//  outside  endpoint  of  CCW  se^nent  46 

if  (tripod  =  0){ 

//  test  footl  in  leg  1  workspace 

footnum  =  1;  //  select  foot  number  one 

//  test  for  intersection  with  outer  workspace  limit 

interceptjlag  “  arcint(afoot,  dir,  cenbod,  outrad,  seglnout,  seglpout,  footnum,  intercepts,  distance); 
if  (intercept_flag  1) 

distance_a  distance; 

//  test  for  intersection  with  CW  workspace  limit 
intercept_flag  =  segint(afoo(,  dir,  seglpin,  seglpout,  intercepts,  distance); 
if  (intercept_flag  =1) 
distance_a  =  distance; 

// test  for  intersection  with  CCW  workspace  limit 
intercept_flsg  =  segint(afoot,  dir,  seglnin,  seglnout,  intercepts,  distance); 
if(intercept_flag“  1) 
distance_a  °  distance; 

//  test  foot3  in  leg  3  workspace 
footnum  =  3  ;  //  select  foot  number  three 
//  test  for  intersection  with  outer  workspace  limit 
intercept_flag  =  arcintfefoot,  dir,  cenbod,  outrad, 

seg3nout,  seg3pout,  footnum,  intercepts,  distance); 

if  (intercept_fUg  =  1) 
distance_b  =  distaiKe; 

// test  for  intersection  with  CW  workspace  limit 
intercept_flag  =  segintfefoot,  dir,  seg3pin,  seg3pout. 
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imercepts,  distance); 

if  (iineTce{il_flag  —  1) 
disUoce^b  **  distanoe; 

//test  for  intersectiaa  with  CCW  woricspace  limit 
iidereept_flag  >  segintfGfoot,  dir,  seg3nin,  segBnout, 

intercepts,  distance); 

if(intereept_fUg“=  1) 
distance_b  =  distance; 

// test  foots  in  leg  S  workspace 
footnum  S;  //  select  foot  number  five 

// test  for  intersection  with  outer  workspace  limit 
interGept_flag  =  arcintfefoot,  dir,  cenbod,  outrad, 

segSnout,  segSpout,  footnum,  intercepts,  distance); 

if(intercept_flag=  1) 
distance_c  >  distance; 

// test  for  intersectioo  with  CW  workspace  limit 
intercept_flag  =  segintfefoot,  dir,  segSpin,  segSpout, 

intercepts,  distance); 

if(intercept_flag=  1) 
distance_c  =*  distance; 

//  test  for  intersection  with  CCW  workspace  limit 
intercept_flag  =  segintfefoot,  dir,  segSnin,  segSnout, 

intercepts,  distance); 

if  (iiitercept_ilag  =1) 
distance_c  =  distance; 

}  //this  ends  tripod  0  testing 

else  if(lripod  =  2){ 

// lest  foots  in  leg  2  workspace 

footnum  “  2;  //  select  foot  number  two 

// test  for  intersection  with  outer  workspace  limit 

intercept_flag  =  arcintfhfoot,  dir,  cenb^  outrad, 

segSnout,  segSpout,  footnum,  intercepts,  distance); 

if  (intetcept_flag  ==  1) 
iistance_a  =  distance; 

// test  for  intersection  with  CW  workspace  limit 
Litercept_flag  =  segintfbfoot,  dir,  se^pin,  segSpout, 

intercepts,  distance); 

if  (inteTcept_flag  =1) 
distance_a  =  distance; 

//  test  for  intersection  with  CCW  workspace  limit 
iiitercept_flag  =  segintfbfoot,  dir,  s^nin,  segSnout, 

intercepts,  distance); 

if  (interceptjlag  =  1) 
distatKe_a  =  distance; 

// test  foot4  in  leg  4  workplace 
footnum  =  4;  //  select  foot  number  four 
//test  for  intersection  with  outer  workspace  limit 
interceptjflag  =  arcintfdfoot,  dir,  cenbod,  outrad. 

seg4nout,  seg4pout,  footnum,  intercepts,  distance); 

if  (intereept_flag  1) 
distance_b  =  distance; 

//  test  for  intersection  with  CW  workspace  limit 
interceptilag  =  segintfdfoot,  dir,  seg4pin,  seg4pout, 

intercepts,  distance); 

if  (intercept_flag  =  1) 
distance_b  =  distance; 


192 


//  test  for  intenection  with  CCW  workspace  limit 
iiiteroept_flag  =■  segint(dfoot,  dir,  segdnin,  seg^Kxit, 

intercepts,  distance); 

if  (intereept_flag  ==  1) 
distance_b  >  distance; 


// test  fi)otd  in  teg  6  woikq>ace 
fixitnum  6; // select  foot  number  six 
// test  fix  intersection  with  outer  workqfMce  limit 

inten;q)t_flag  =  arcint(ffoot,  dir,  cenbod,  outiad,  segdnout,  seg6pout, 
footnum,  intercepts,  distance); 

if  (intercept_flag  =  1) 
distanoe_c  °  distance; 


// test  for  intersection  with  CW  workspace  limit 
inteicept_flag  =  segintfffoot,  dir,  segdpin,  segdpout, 

intercepts,  distance); 

if(intercept_flag=  1) 
distaiice_c  =  distance; 

//  test  for  intersection  with  CCW  workspace  limit 
intercept^flag  =  segint(£foot,  dir,  seg6nin,  segdtMut, 

intercepts,  distance); 

if  (intercept_f]ag  =1) 
distaoce_c  >  distance; 


}  //this  ends  tripod  1  testing 

mavdict  =  min(min(distance_a,  distance_b),  distance_c); 


return  (maxdist); 

}  //  this  ends  fimction  maxdistance_4Scmfoot 

//.*«**.*.*«**«**.««»«*»;VPS*aiid*PHRI********************‘************ 

//TITLE;  arcintO 

//INPUT:  (x,y)body  coordinates  ofa  selected  foot;  direction;  body  center, 

//  radius  ofworkspace;  CCW  arc  segment  en(^int$;CW  arc  segment 

//  endpoints;  foot  number  selected 

//OUTPUT:  intescepts  of  arc  segment;  mdistance  to  the  intercepts  fiom  the 
//  current  foot  position;  flag  to  indicate  whether  an  intercept 

//  was  found 

//FUNCTION;  This  function  determines  whether  there  is  an  intercept  of  the 
//  desired  direction  ofthe  foot  with  an  arc  segment  defining  a  part 

//  of  the  workspace. 

//«»•••••••«»••••••«»»»»  AQUAROBOT*********************************** 

int  arcintfdouble  footQ,  double  direction,  double  cenbod[],  double  radius,  double  negJimitQ,  double  pos_limit[],  int  foot_number, 
double  interceptsQ,  double  &mdistance) 

{ 

//initialization 

double  a,  b,  c,  d,  x_intercept,  y_intetcept; 
double  perp_di^ance,  len^  x_petp,  y_petp; 
double  rad_s({uared,  perp_squared; 
double  radius_test,  in_direction,  out_direction; 
double  atc_fi)ot,  neg_arc_limit,  pos_atc_Iimit; 
int  arcsegment_flag  =  0; 


peip_distance  =  ((cenbod[4]  -  foot[s])*cos(direction)) 
-  ((cenbod(3]  -  foot[0])»siri(direction)); 
(perp_distance  <=  radius){ 
radsquared  =  radius*radius; 
perp_squared  =  perp_distaiKe*peip_distance; 
if  (p^_distance  <  0) 

length  =  sqrt  (-(rad_squared  -  perp_squared)); 
else 
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Icogih  ■>  sqft(r»l_sqtured  -  pap_squared); 

// find  (x,y)  at  intenectioo  of  peiptndicuUr  mdisUnce  and  length 
x_pcfp  =  oenbod{3]  +  (pap_diitance*sin(directioD)); 
y_peip  =  cenbod[4]  -  (peq>_distance*oiM(<iirectioa)X 

// test  if  foot  is  inside  or  outside  radius 
a  »  (foot(l]  -  oenbod{4])*(foat[l]  -  cenbod{4]); 
b  «  (fi>ot{0]  -  oeidMd[3])*(fi>ot[0]  •  codradp]); 
radius_test »  sqrt(a  b); 

if  (radius_test  >  iadius){ 

//  find  (x,y)  at  intersection  of  ray  and  arc  if  foot  is  outside  radius 
x_inletccpt  >  x_pen>  -  (Ic<igdi*oos(diieclion)X 
y^intercept  =  y_petp  -  Ocnglh*sin(directian)X 
iiiteroepts[0]  -  x^inlercept; 
interoqits{lj  °  y_intercc{it; 

> 

else  { 

// find  (x,y)  intersection  of  ray  and  arc  if  fi>ot  is  inside  radius 
x_iiderccpt  >=  x_perp  +  (lenglh*oos(direction)); 
y_intercept  =  yj>*rp  +  Oenglh*$in(direction)); 
intetoe|its[0]  =  x^intercept; 
interce|)ts(l]  =  y_intetcept; 

}  //radius  test  ends  here 

// test  if  intersection  is  in  desired  direction 

in_direction  =  fi>ot[0]*oos(diiectian)  foot[l]*sin(direction); 

out_directioo  *  x_iiitercept*cos(directioo)  y_intetcetit*sin(directionX 

if  (in_diiection  <=  out_direction){ 
c  *  ^_interoept  -  foot[l])*(y_interoeiit  -  foot[l]); 
d  =  (x_intercept  -  foot{0])*(x_interce|it  -  fix>t[0]); 
mdistance  =  sqrt(c  (Q; 


// test  if  intersection  it  HTtfain  arc  segment 
arc_fi)ot  *=  atan2(y_intercept  •  cenbod[4],  x_interce|X  -  cenbod[3]X 
iieg_arc_liinit  atan2(neg_limit[l]  •  oeiibod(4],  neg_liiiut[0)  •  cenbod[3]}; 
p(K_arc_Uniit  =  ataii2(pos_liinit[l]  -  cenbod[4],  po$_limit[0]  -  cenbod(3]); 

// tests  to  ensure  proper  sections  of  arcs  are  used 
if  (foot_ouinber  =  1){ 

if  (arc_fi>ot  >=  neg_arc_Iimit  arc_fbot  <=  pos_arc_Iiinit) 

arcsegaieiit_fUg  =  1; 

} 

else  { 

if  (direction  >  0){ 
if  (neg_arc_liinit  <  0) 
neg;_arc_liitiit  “  neg_arc_limit  +  (2*PI); 
if  (]pos_arc_liinit  <  0) 
pos_arc_limit  =  pos_arc_liiiiit  +  (2*PI); 
if  (aic_foot  <  0) 
arc_foot  =  arc_foot  +  (2*PI); 

if  (arc_foot  >=  neg_arc_limit  &&  arc_foot  <=  pos_arc_limit) 
arcsegnient_fiag  =  1; , 

} 

else  {  //  direction  <  0 
if  (neg;_arc_limit  >  0) 
neg_arcjimit  =  neg_arc_limit  -  (2*PI); 
if  (pos_arc_liiiBt  >  0) 
pos_arc_liinit  =  pos_arc_iiinit  -  (2*PI); 
if  (arc_foot  >  0) 
arc_foot  =  arc_foot  -  (2*PI); 

if  (arc_fi>ot  >=  neg_atc_limit  &&.  arc_foot  <=  pos_arc_liiiiit) 
arcsegnMnt_flag  =  1; 

}  //direction  test  ends  here 
)  // foot  nuinber  test  eixls  here 


194 


}  //inicnection  tot  cads  here 
}// perpindicuUr  test  ends  here 


if(arcsegiiient_iUg  !=  1){ 
arGsegmeat_fUg  -  0; 

}  //  nullifying  panunelers  ends  here 

return  (srcsegnient_flsg); 

}  //arcintfimctian  ends  here 

//.*.*****««.*«*«***«***««^*^*PHSi***«»«««***<»*«***«*«***< 

//REVISED:  . 

//TITLE:  segintO 

//INPITT :  (x,y)body  coordinates  of  a  selected  foot;  direction;  inner 
//  endpoints;  outer  endpoints 

//OUTPUT:  intercepts  of  line  segment;  mdistance  to  the  intercepts  from  the 
//  current  foot  position;  flag  to  indicate  whether  an  intercept 
//  was  found 

//FUKCnON:  This  function  determines  whether  there  is  an  intercept  of  the 
//  desired  direction  of  the  foot  with  s.'-<se  segment  defining  a  part 

//  ofthe  woflcqiace. 

//*,.*«».....««.«.«..««^qUaroBOT**********************«****** 

int  segint(doubte  firotQ,  double  direction,  double  insegQ,  double  outsegQ, 
double  interceptsQ,  double  &indistance) 

{ 

//initialization 

double  a,  b,  c,  d,  theta_segment,  theta_di£ferenoe; 
double  x_oumerator,  y_nunierstor,  denominator, 
double  x_inletcept,  y_intetoept; 
double  segment_test,  CCW,  CW,  point_l,  point_2; 
im  linesegment_ilag  0; 


// find  theta  of  line  segment 

tbeta_segtnent  ^  atan2(outseg{l]  •  insegfl],  oatseg[0]  -  insegfO}}; 
tbsta_difrerence  ==  direction  •  theta_segment; 


//  if  there  is  no  difiference  between  direGtioo  ray  and  the  orientation  of 
// the  line  segment  being  tested,  then  they  ate  parallel;  no  intersection 
if (  tbeta_diffeteace  !=  0.0 
tbeta_diffeteiice  !=  PI 
&&  theta_difference  !>  -PI){ 

b  -  (inseg{0]*sin(theta_segmeiit))  •  (itiseg[l]*cos(theta_segment)); 

a  >  (foot[0]*sin(ditection))  -  (fo^l]*oos(direction)); 

x_Dunietator  =  (•oos(theta_segment)*a)  (cos(direction)*b); 

y_iiuineiator  »  (sin(direction)*b)  -  ($in(theta_segment)*a); 

denominator  =  tiii(theta_segment  -  direction); 

x_in^xcept  =  x jnumerator  /  denominator, 

y_intercept  ^  y^numerator  /  denominator, 

intercepts[0]  ==  x_intercept; 

intercepts[l]  =  y_interGept; 


//  determine  if  the  direction  ray  intersects  the  line  segment 

CCW  =  (inseg[0]*cos(theta_segmeiit)  +  inseg[l]*sin(theta_segnieitt)); 

CW  =  (outseg(0]*cos(thela_segment)  +  outseg(ll*sin(theta_segment)); 
segment_test  (x_intercept*cos(theta_segment) 

y_intercept*sin(theta_segment)); 

if  ((CCW  <=  segment_test )  Sc&  (CW  >=  segment_test)){  //  if  true,  intersection 
//  determine  if  the  orientation  is  correct 

point_2  =  (x_intercept*cos(direction)  +  y_intercept*sin(direction)); 
point_l  =  (foot[0]*c^direction)  +  foot[l]*sin(direction)); 

if  (point_2  >  point_l){ 

//  correct  orientation 
linesegment_flag  =  1; 
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c  •  (y_inicroept  -  fooi(l])*(y_iniaiEepl  -  foot{l]); 
d  >  (x_ioleroept  -  foal{0])*(x_inlerocpl  -  foot(0]); 
mdktancif  -  s^c  +  dy, 

}  //orienutioa  tot  eodi  here 

}// line  segment  inlenection  test  endi  here 

}  //  line  intenectioateM  ends  here 

// if  one  of  the  tests  &ib,  there  is  no  nXenectioa 
if  (liuuepnent_fl«g !-  1){ 
linesegment_fUg  0; 

}  // nullifying  panmeten  ends  here 

return  (linesegnient_flag); 

}  II  segint  fiiitctioa  ends  here 

#ifiidefARn;NC_H 

MefineARFUNC.H 

//  FILENAME:  srfiinc.H 
//PURPOSE. 

//  CXIMMENT:  include  file 

#include  'atcoosLH* 

^include  'Kineinstics.H' 


extern 

double  min(double,  double); 
extern 

double  max(double,  double); 
extern 

void  eUipte(WelkP«rameter  &,  double  *,  double  *); 
extern 

void  kinematicsfdouble*,  double*,  double*,  double*,  double*); 


extern 

voidim'  '  inemstictfdouble*,  double*); 


extern 

void  world_body(doubie*,  double*,  double*); 
extern 

void  body_warld(doubte*,  double*,  double*); 
extern 

int  srcint(double*.  double,  double*,  double,  double*.  double*,iM,double*,  double  &); 
extern 

int  segiid(double*,  double,  double*,  double*,  double*,  double  &,  int); 
extern 

double  maxdistance_2Scinfoot(Next_Motioo  &,  WalkParameler  A), 
extern 

double  nuxdistence_4Scinfoot(Nexl_Motion  A,  WaOcParanieter  A); 

#endif 

//  FILENAME:  artpgsitC 
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//PURPOSE:  tripod  gut  using  ellipte  trajectory 

//COMMENT:  to  mske  know  the  motor  speed. 


^include  <stdio.h> 

#inciude  <slreanUi> 

^include  <math.h> 

Mnchide  <stdlibJi> 

^include  'ercoostH* 
iiinclude  '«iiuiic.H* 

#inchide  ’ertpgait.H' 

#iiiclude  *Kineniatics.H* 

//  FUNCTION:  set_goalO 

void  get_goel(douUe  goslQ) 

{ 

printfl^Goal  point(  x  y  )==>"); 
scufC’ftif  Kir,  AgoalpCW],  &goel[YW]); 

}//  end  of  get_goalO 

//  FUNCTION:  get_aienuO 

int  get_nienu(int  menu) 

{ 

printfr'O...Discrete  Motionin'*); 
printIt'l-'-CoQtinuous  Motion  ■=>*); 
scuifi7%d",  Amenu); 


return  (menu); 

}//  end  of  get_tneaaO 

// FUNCTION:  get  footchoiccQ  (uUed  06May93) 

int  get  footcfaoiccfint  footsize) 

{ 

priiitf(''0...23  cm  diuneter  fbotpad\n"); 
printf(*1...4S  cm  diuneter  footpad  “=>"); 
scanflT^iir,  Afoolsize); 

return  (footsize); 

}//  end  of  get_footchoiceO 

//  FUNCTION:  gait_algotittaoO 

void  gait_algorifhm(Nexl_Motion  Aactual, 
double  goalQ, 
int  menu, 

int  footsize)  //  selecting  discrete  or  continuous  motion 

{ 

static  WalkParameter  wp; 
static  Flag  flag; 
static  int  counter. 


switch  (menu)  { 
caseO:  // discrete  motion 
/•set  flag*/ 

diac_set_flag(actual,  wp,  goal,  flag); 
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/*  gAltjtiUBUIlg  */ 
if  (flag.phtie£ad  **  1)  { 

disc_giit_pbiaiiii8(«ctual.  wp,  fUg,  goal  footiae); 

} 

/*  Motion  Ptaoning  */ 
diicJripod_matiaa(actiul,  wp,  flag); 

bteak; 

easel:  // conliiaious motion 
/•set  flag*/ 

cont_set_flag(artiial,  wp,  goal,  flag); 

/*  gait_planoing  */ 
if  (fUgphaseEod  *=*  1)  { 
oont_gait_plamiing(aonial,  wp,  flag,  goal); 

} 

/*  Motion  Planning  */ 
coiit_tripod_motion(actual,  wp,  flag); 

break; 

de&utt 

break; 

) 

//  ptiitt_wa]kpafa(wp); 

}//  End  of  gait_algorithmO 

//  FUNCTION:  robot_modeiO 

void  robot  model(Next  Motion  ftnm) 

{ 

double  euler(3];  //  Euler  angle 

double  bodypj;  //  Body  Position 

double  jangle[L£G](JOINT];  //Joint  angle 
double  jpotnt^XG][JOINT]pCYZ];  //  Joint  Position 
int  leg,  joint,  axis; 


/*  Initialize  */ 

eukrfO]  -  nRLbody_oenter_coord[0]; 
euler{l]  -  inLbody_ccnler_ooard[l  j; 
euler(2j « ini.body_cenleT_oootd(2]; 
bodyfxW]  -  nni.body_center_cootdp]; 
body[YW]  *  nmbody_oenter_coord[4]; 
bodyjZW] »  noLbody_oenter_coord[S]; 

/*  joint  angle  initialize  */ 
for  (leg>LEGl;  leg<=LEGd;  leg^^)  { 
for(joint”CB;joint<“FOOT;joint-t-+)  { 
jangle(leg][joint]  -  nm.iid>d Joint  angle(leg][joint]; 

//Tricky  part!  5ibdJoiiit_angleQ[HIP->KNEE2](HlP,KNl,KN2,FT} 
//Tricky  part!  janglea[CB->KNEE2]{CB,HIP,KNl,KN2,FT) 

} 

> 

for  Oeg=LEGl;  leg<-LEG«;  leg^-t-)  { 

/*  Kinematics  computation  for  GNU  plot  data  file  */ 
kineniatics(jangle(leg],  jpointneglpriP),  jpoiiit[leg](KNEEl], 

jpointileg]  [KNEE2  J,  jpoint[leg]  [FOOT]); 
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for(ioint=°HIP;  join»<*FOOT;  joiHt++){ 

/*  Coordniale  TnnsfannitionCWORLXK-BOOY)  */ 
warid_body<eiiler,  body,  jpoint[le^[joiflt]); 

} 

) 

/*  set  Next_Mo(ioii  •/ 

for  (axis=XW;  •xis<»ZW;  «xi*++)  { 

aiafoot_l_coord{axis]  ~  jpoiiit[LEGijiFOOT](udf]; 
iiin.foot_2_coatd{axu]  -  jpoiiit(L£G2j[FOOT](axisj; 
nmjfoot_3_coord(«xjsi »  jpobittLEG3i[FOOT][«xi*j; 
iiiiiJbot_4_coocd[axisj  -  jpoiiitlL£G4j(FOOT)(axis]; 
iim.foot_3_ooord[axis] »  jpoiiit[LEG3j(FOOT][axaj; 
niiLfoot_6_coord{axis]  ~  jpoint[L£G^j[FOOT][axit]; 

> 

/*  set  contact  fUg.*/ 
if  (niiUbot_l_coord[ZW]  >-  -0.01)  { 

nm.leg;_coiitact_iUg(L£Gl]  •  1;  //  set  contact  flag(Legl) 
am-foot  l_coord(ZW]  0.0; 

} 

else  ( 

nm.Ieg_coatact  flag{L£Gl]  =  0;  //  reset  contact  flag 

} 

if  (nm.foot_2_coord(ZW]  >-  -0.01)  { 
iim.leg_oontact_flag(LEG2]  >  1;  //  set  contact  flag(Leg^) 
nmibot  2  coord(ZW]  -  0.0; 

} 

else  { 

nm.leg_contact  fIag(LEG2]  >  0;  //  reset  contact  flag 

} 

if  (nmibot_3_coord[ZW]  >-  -0.01)  { 

nm.leg_oontact_flag[LEG3]  =■  1;  //  set  contact  flag(Leg3) 
nm.foot  3  ooord[ZW]  =  0.0; 

} 

else  { 

nm.leg_contact  flag[LEG3]  <■  0;  //  reset  contact  flag 

} 

if  (nnvfoot_4_coord[ZW]  >=  -0.01)  { 
nm.leg_cootact_fla^LEG4J  =  1;  //  set  contact  flag(Leg4) 

} 

else  { 

nm.leg_contact  flag[LEG4]  =  0;  //  reset  contact  flag 

> 

if  (nm.foot_5_coord(ZW]  >=  -0.01)  { 

nm.leg_cootact_flag[LEGS]  =  1;  //  set  contact  flag(LegS) 
nni.foot_5_coord[ZW]  =7).0; 

} 

else  ( 

nm.leg_contact_flag[LEG3]  =  0;  //  reset  contact  flag 

> 

if  (nm.foot_6_coord[ZW]  >=  -0.01)  { 
nm.leg_contact_flag[LEG6]  =  1;  //  set  contact  fIag(Leg6) 
nm.foot_6_coord[ZW]  =  0.0; 

} 

else  { 

nm.leg_contacl_flag|LEG6]  =  0;  //  reset  contact  flag 

} 

)//  End  of  robot_niodel0 
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//TITLE:  diic_fet_£UgO; 

//Fl/NCnON:  taiiOagt. 

//  INPUT:  phaie,  Next_Motiao  cUm,  Flag  class. 

//OUTPUT:  Flag  dan 
//  CALLED  BY :  gait_algaritlanO 
//CALLS:  Nooe 
//COMMENT:  fiir discrete motioo. 

H  *••••*••••••••••••••••*••«•***«•««««••«•««*••*«••••*••••••••«••**•••*« 

void  disc_iet_flag(Next_Motioii  &an, 

WaOcParaoMlCT  &wp, 
double  goalQ, 
FlagAfiag) 

{ 

double  distance; 

/*  set  phase  end  flag  */ 
if((wp.phase"0)&&  //TPO phase 

(nm.leg;_contact_flag[l-EGl] "  1)  dMk  //  TPO  cootuct 
(flag-phaseEnd  — °  0))  { 
flag.phaseEnd  *  1; 

} 

elseif((wp.phase“2)&A  //TPl  phase 

(iiin.leg_oocitact_flag[LEG4]  1) && //TPl  contuct 
(flag-phaseEnd  -=  0))  { 
flag.phaseEnd  °  1; 

} 

else  if  ( (wp.phase  BODYO)  &&  //  BODY  phase 

(fibs(nm.body_cenler_ooord[3]  -  (nm.foot_l  coordpC]  -  TripodSize)) 

<1.0)&& 

(flag.phaseEnd  =  0)  )  { 

flagphaseEnd-  1; 

} 

else  if  ( (wn-phase  BODYl)  &&.  //  BODY  phase 

(fths(imi.body  center  co^P]  •  (miLfoot  4  coord[X]  TripodSize)) 

<1.0)&& 

(flag-phaseEnd  =  0)  )  { 

flagphascEnd  •>  1; 

} 

else{ 

flag.phaseEnd  =  0; 

} 

/•Set  GOAL  flag*/ 

distance  =  sqtt(  (nin.body_ceater_ooord[3]  •  goaipiW]) 

•(nm.body_cenler_cootd(3]  -  goal(XW]) 

+  (iiin.bo^_center_ooord[4]  -  goalfYWJ) 
•(nm.body_center_oootd[4]  -  goalfYW])  y, 

if  (distance  <«  wp.bodyspeed)  { 
flagphaseEnd  1; 
flaggoal  =  1; 

} 

}//  End  of  disc_set_flag0 

//•.•*****.••*•**••••••••••*•*••*•*••••*•••«•••***•***•*•****••••**•*•>* 

//TITLE:  disc_gait_planningO 

//  FUNCTION:  Gait  planning  This  fimction  determins  walking  parameters. 

//  INPUT:  motion  phase,  WalkParameter  class,  Flag  class. 

//  OUTPUT:  WalkParameter  class. 

//RETURN:  motion  phase. 

//  CALLED  BY :  mainO- 
//  CAIXS:  None 
//COMMENT:  for  discrete  motion 

//*«•**••*•*•«••*•••*••••**«••*••**••••**•**••••*•******••••*•••*•**••• 

void  disc_gait_planning(Next_Motion  &nm,  // 

WalkParameter  &wp// 
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FUgABMg,  I  I 

double  goaipOQ, 
int  footsize)  //  g(^  poeitioo 


{ 

static  int  counter  =  0; // to  count  phase  number 

double  distance;  //  distance  between  START  and  GOAL 


!*  goal  case  •/ 
if(flag.goal  =  l){ 
exit(0); 

} 

/*  to  find  direction  */ 

wp.direction  *  atan2(goal(YW]  -  nnLbody_ceater_coord[4], 

goal|XW]  •  imi.b(^_center_coanl[3]y(DR);  //  in  degrees 

/*  to  determine  a  stride  */ 
if  (couitter  =  0)  { 

printlf*\n"); 

printirPHASEO  =  %dto“,  wp.phase); 
if  (footsize  =  0)  { 

wp.stri^  =  maxdistance_2}cmfoot(nm,  wpX 

} 

else  { 

wp.stride  =  maxdistance_4Scinfoot(nm,  wp); 

} 

} 

eise{ 

/*  increment  phase  */ 
wp.phase  wp.|duse  +  1; 
wp.phase  wp.phase  %  4; 
print^THASE  =  %d\n",  wp.phase); 

if(footsize  =  0)  { 

(  (wp.phase  “  0)  8  (wp.phase  =  2)  )  { 
wp.stride  >=  maxdistance  25cmfoot(nra,  wp); 

-  } 

else  { 

if  (counter  ==  1)  { 

wp.bodyspeed  O.S; 

} 

else  ( 

wp.bodyspeed  =  0.3; 

) 

} 

> 

else  { 

//this  area  is  for  43cm  fyotpad,  duplicate  23cm  footpad  routines 

} 

} 

eounter++; 

/*  to  find  distance  between  cunent  position  and  GOAL  */ 
distance  =  sqtt(  (goal[XW]  -  nm.body_center_ooord[3]) 

*(goaipCW]  -  nm.body_center_coord[3]) 

+  (goal[YW]  -  nm.body_center_coord[4]) 

*(goal[YW]  -  nm.body_center_eoord[4])); 

/*  handlingincaseofrobot  is  near  the  goal  */ 
if  (distance  <  (wp.stride/2.0))  { 
wp.stride  =  2.0*distance; 

} 
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}/*  Ei^  of  disc _g*it_pluniDgO 


//  FUNCTION:  ^sc  ti9od_iD0liaa0 
//PURPOSE:  discr^ tripod moliaa. 

//  to  compute  one  inctcinsntel  usotioa  ustng  ellipK  tnjectoty. 

//  to  calculate  one  inameoied  joint  statuses 

//  INPUT:  class  Nexl_Motioii, 

//OUTPUT:  class Next_Motioo 
//COMMENT:  called  by  oiock»_plaiaimgO- 

void  disc_tripod_inotiaa(Next_Motiaa  drim, 

WalkPsranteter  dtwp, 

FlagAflag) 

double  eulesflbreeDim],  oldeolertThreeDim]^/  Euler  angles 

double  deulerTTIireeOim];  // Chai^  in  Euler  an^e 

double  bodyfTlaeeDim],  oUbodyflbreeDim];  //  CB  positioD  on  World 

double  dbodyfThreeDim];  //  Chai^  in  BODY  position 

double oliya^[L£G][K)INTI;  //old joint  angles 

double  jang|e[LEG][JOINT];  //joim  angles 

double  jpoint[L£G](X>INT](ThreeDiffl];  //joint  positioiis 

static  double  foolptiBt(LEG][TbreeDiffl];  // foot  priit(cootacte<i  point) 

double  dbm; 

int  le&  jotnt,  axis; 

/*  Initialization  */ 

for  (axis«XW;  axis<“ZW;  axis++)  { 

/*euler  angles  are  not  needed  for  ver.  1.0  */ 

euler[axis]  “  oldeulerjaxis]  =  nm.body_oeoter_coofd(axis]; 

bodyftudsj  ~  otdbody(axis]  =  om.body_ceiiter_coofd(axis+3J; 

/*  Feet  position  initialize  */ 

jpointILEGl](FOOT][axis]  •  nm.foot_l_ooordlaxis); 
jpoint[LEG2][FOOTl(*^i  *  iwt^_2_coord(axis]; 
jpoint(LEG3Jp=OOTI(axisj  -nraibot_3_coord(axis]; 
jpoint[LEG4j(FOOT)[axis]  =>  itnLfoot_4_eoord(axis]; 
jpoiiit[LEG5](FOOTl(axisi  =  nm.foot_5_coord{axis]; 
jpoint[LEG«][FOOT]iaxis]  •  mn.foot_6_coord{axisj; 

/*  footprint  initialize  */ 
if  (flag.pfaaseEnd  1)  { 

if  Inm-leg  contact  flagfLEGlI  "1) 
footpriiil(LEGl][axis]  >  onLfoot_l_coo(d{axis]. 
if  (nm.leg_conlact_flag(LEG2]  =  1) 
footpriiit(L£G2][axis]  -  nm.foot_2_eoord[axis]; 
if(nm.leg_oontact_flag(LEG3]  =  1) 
footprint[LEG3][axis]  =  nmJbot_3_coord[axis); 
if  (nm.leg_contact_flag(LEG4]  =  1) 
footpriiit{L£G4][axis]  -  nmibot_4_ooord(axis]; 
if  (nni.leg_eoiitact_flaglLEG5J  “  1) 
footprint[LEGS][axis]  -  nm.foot_5_coord[axisj; 
if  (nm.leg_contact_flag(LEG6]  ”=  1) 
footpriilt[L£G6](axis]  =  nm.foot_6_coord[axis]; 

} 

> 

/*  Joint  angle  initialize  */ 
for  (leg=LEGl;  leg<=LEG6;  legt-t-)  { 
for  (joint=CB;  jonrt<=FOOT;  joint++)  { 
jangle(leg][joint]  =  nm.inbdJoint_angle[leg][joint]; 
oldjang]e{leg]{joint]  =  jangle{leg][/oint]; 

//Tricky  part!  inbd_joint_angleQ[HIP->KNEE2){HlP,KNl,KN2} 
//Tricky  part!  jangleG[CB->KNEE2){CB,HIP,KNl,KN2> 

} 

> 
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if  ((wp.piuict€2)  ^  1)  { //  if  BODY  pinie,  tiien  increment  body  poeition 
dfam  =  wp.bodyspced; 


/*  to  incnment  body  poatico  */ 
bodypCW]  <l)m*ooa(wp.direcliao*DR); 
bodyfYW]  ■*=  dbm*sin(wp.diRctkn*OR); 
bo<^lZW]+=0.0; 

} 

/*  to  calculile  joint  angla  */ 

for  (Ieg=LEGl;  leg<=LEG<;  lef*-+)  { 

if(  ((wp.phase  ^  0)  &A  ((teg%2)  =  0))  //tpO  AND  odd  leg# 

B((wp.ph»*e“  2)  &&((leg%2) -“!))){  //tpl  AND  even  leg# 

/*  compute  FOOT  potitioa  on  <n  ellipce  curve  respect  to  WORLD  */ 
ellipsefwp,  footprintfleg],  jpoint[leg](FOOT]X 

/*  Foot  CooUct  Conditioa*/ 
if  (ipoii#(leg|[FOOT](ZW]  >  0.0)  ( 
jpowtllegJpXXXTjpCW]  =•  idotpriitpeg][XW] 

+  vvpjtTidc*cai(wp.direclion*DRX 
jpoii«tneg][FOOT][YW]  -  footprii#{leg][YW] 
wp.stride*sin(wp.direction*DR); 
jpoint(teg][FOOT)(ZW]-footprintpeg][ZW);  //O.O; 

> 

} 

/•  Coordinites  Tniisfoni>>tioa(BODY<-WORLD)  •/ 
body_warld(euler,  body,  jpointneg][FOOT]); 

/*  Inverse  Kinematics  */ 

inv  kineinatics(jpoint[leg][FOOT],  jangle(teg]); 

} 

/*  set  joint  angle  */ 

foi(joint><B;  joint<*KNEE2;  jomt++){ 

fof<leg-LEGl;  leg<-LEG<;  legv*){ 
nm-inbdjoint  angle[leg][joint]  ~ jangle[leg][ioinl]; 

//Tridcy  part!  ^ Joint  angleQ(HIP->KN£E2](HIP.KNI,KN2,FT} 

//Tricky  part!  jangieO[CB->KNEE2]{CB,HIPJ<NlJCN2,FT} 

} 

} 

/*  to  set  next  status  */ 
nin.body_center_CDord(3]  =  bodypCW]; 
nm.body_center_coord(4]  •  bodyfYW]; 
nm.body_certer_coord[5  j  =  bodyfZW]; 

/*  to  calculate  duuige  in  body  position  and  euler  angles  */ 
for  (axis=XW;  axis<=ZW;  axis-H-)  { 

deulerfaxis]  =  eulerfaxis]  -  oldeulerfaxis]; 
dbodyfaxis]  =  bodyfaxis]  -  oldbodyfaxisj; 

} 

/*  to  calculate  diange  in  BODY  position  */ 
nm.bodyniotioa[0]  >  deulerf  AZIMUTH]; 
nm.bodymotion[l]  =  deulerfELEVATION]; 
iinLbodymotioo[2]  =  deulerfROLL]; 
nm.bodyniotion[3]  dbodypCW]; 
nm.bodyinotion[4]  =  dbodyfYW]; 
nm.bodynK>tion[S]  =  dbodyfZW]; 


/*  to  calculate  change  in  joint  angles  */ 
foffjoint=HIP;joint<=KNEE2;joint++){ 

iim.leglinotion[joiiit-l]  >jangle[LEGl][joint]  -oldjangle[LEGl][joint]; 
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Bm.leg2inoli<iii[>oiiit-1]  -  jaiigle[L£C2][|oint]  -  oldiaogle(L£G2][jotot]; 
i)m.leg3matioii{io^-lj  >  j«iigle[LEG3j[)oiiili  -  oUyaiig|e(L£G3][]oint]; 
nm.leg4iiiolioa[joilat-l]  ~  jaiiglc[L£G4][joint]  -  ol4iingle[LEG4j[joint]; 
nalcg5ino(iaa(ioint-lj  >  jangkpjEGsjyoint]  -  ol(4aiigtc(L£Gs][joint]; 
l■n.leg6lDo(ioo[)oint-l]  =■  jaiiglc(LEG6]£joint]  -  okyan^(L£G<][jomt]; 

} 

}/*  End  of  duc_tr^>od_mo(ioo  */ 

//TITLE:  coDt_tct_flaeO; 

//FUNCTION:  tosctlUg*. 

//INPUT:  phase,  Next_Mo(i<n  cUa,  Flag  clast. 

//OUTPUT:  FUg  class 
//  CALLED  BY :  gait_algaritlmO 
//CALLS:  None 

//COMMENT:  for coidinuous motion. 

void  coat_set_flag(Next_Motian  Am, 

WaUcParameler  Awp, 
double  goal(]. 

Flag&flag) 

{ 

double  distance; 

/*  set  phase  end  flag  */ 

if  ( (wp.phaae  0)  AA  //  TPO  phase 

(nfn.le^oontact_flaig{LEGl]  "1)  AA  //  TPO  contuct 
(flag.phaseEod  —  0))  { 
flag.ph^End  -  1; 

} 

else  if  ( (wp.phase  ***  1)  AA  //  TPl  phase 

(nm.leg_contact_flag(LEG4]  1)  AA  //  TPl  contuct 
(flag.pltateEnd  0))  { 
flag.phaseEiid  >•  1; 

) 

else  ( 

fla^phaseEnd  0; 

} 

/•Set  GOAL  flag*/ 

distance  ~  sqrt(  (nni.body_ccnter_coacd(3]  •  goaipCW]) 

*(nm.body_oenter_coord[3}  -  goaipcW]) 

+  (inLbody_oenter_coord[4]  •  goalfYW]) 
•(nni.body_cen!er_coofdl4]  -  goa][YW])  y, 

if  (distance  <»  wp.bodyspeed)  { 
flag.phaseEnd  -  1; 
flag.goal  =>  1; 

) 

}//  End  of  set_flag0 
//  TITLE:  gait_planningO 

//  FUNCTION:  Gait  planning  module.  This  function  determins  walking  panmeten. 
//INPUT:  motion  phase,  WalkParameter  class.  Flag  class. 

//OUTPUT:  WaUcPaiameter  class 
//  RETURN:  motion  phase. 

//CALLED  BY:  mainO. 

//CALLS:  None 

//COMMENT:  for  continuous  motion 

void  cont_gait_planning(Nexl_Motion  Anm,  // 

WalkParameter  ScwpM 

Flag  Aflag,  // 

double  goaipCY])  // goal  position 
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{ 

static  iot  couider^;  //  to  cotmt  phase  number 

double  distance;  //  distance  between  START  and  GOAL 

/*  goal  case  */ 
if  (flag.goal  >==•  1)  { 
exit(0); 

} 

/*  to  detennioe  a  stride  */ 
if  (counter  “  0)  ( 
wp.stride  DdaultSTRIDE/2.0; 

wp.bodyspeed-((De£uiltSTRIDE/2.0yFINEy2.0; 

} 

else  { 

wp.stride  =  DcfauHSTRIDE; 
wp.bodyspecd=(DefaultSTRIOE/2.0yFINE; 

/*  increment  phase  */ 
wp.phase  >  wp.phase  1; 

wp.phase  wp.phase  %  2;  //  continuous:2,  discrete:4 

counter++; 

/*  to  find  distance  between  curroit  positian  and  GOAL  */ 
distance  aqtt(  (goaI[XW]  -  nm.body_center_coord[3]) 

*(go^pCW]  -  nm.body_centsr_soard[3]) 

(goal[VW]  -  anLbody_center_coord[4]) 

•(goal[YW]  •  nin.body_center_coord(4])); 

/*  handling  in  case  of  robot  is  near  the  goal  */ 
if  (distance  <  (wpjtiide/2.0))  { 
wp.stride  ~  2.0*distance; 

} 

/*  to  find  direction  */ 

wp.direction  =  atan2(goal[YW]  -  nm.body_center_coord[4], 

goaipCW]  -  nm.body_cenler_coord[3)y(DR);  //  degree 

}/*  End  of  gait_plannmgO  */ 

//  FUNCTION;  cont_tripod_mationO 
//PURPOSE:  oontinuous tripod motioa 

//  to  compute  one  incranental  motion  using  ellipse  trajectory. 

//  to  calculate  one  incremented  joiid  statuses 

//  INPUT;  class  Next_Motion, 

//  OUTPUT ;  class  Next_Motion 
//COMMENT:  called  by  gait_planningO' 

void  cont_tripod_nK)tion(Ne>!t_Motion  &nm, 

WalkParameter  &wp, 

Flag&flag) 

{ 

double  euler(ThreeDim].  oldeulerfThreeDim],//  Euler  angles 

double  deulerfThreeDim];  //  Change  in  Euler  angle 

double  bodyflhreeDim],  oldbodyfThreeOim];  //  CB  position  on  World 

double  dbodyfThreeDim];  //  Change  in  BODY  position 

double  oIdjangle(LEG][JOINT];  //old  joint  angles 

double  janglc[LEG][fOINT];  //  joint  angles 

double  jpoint{LEG]  [X)INT]  [ThreeDim];  //  joitd  positions 

static  dcuble  foo4)ritit[LEG][ThreeDim];  // foot  print(contacted  point) 

double  dbm; 

int  le£  joint,  axis; 

/•  Initialize  •/ 

for  (axis*XW;  axis<=ZW;  axis-M-)  { 
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/*  oiler  eagles  ere  oot  needed  for  ver.1.0 

oilerfexis]  •  oIdeuler{axis]  -  noi.body_caiter_ooord(exis]; 

body[exis]  -  ol(B>ady[exis]  >iim.bo^_ccnter_coatd[exis't-3]; 

/*  Feet  position  initialize  */ 
jlioii4LEGl][FOOT][exis] » iinLfoot_l_coord[4xis]; 
jpoin5LEG2][FOOTl[exii] «  nm.feot_2_coord[exis]; 
jpoint[LEG3j[FOOT](exb]  -  nm.foot_3_coordlexis]; 
jpoint(LEG4][FOOT][exisi  -=  nniibot_4_ooord[exisl; 
jpoint(LEG5)[FOOT][exisi  -  nm.foot_S_coord(exis]; 
jpoiiitILEGd][FOOT](e)ds]  =  niafi)0t_6_coordlexis]; 

/*  footprint  inhielize  */ 
if(flag.phaseEnd  =  1)  { 

if  (nni.leg_coiitect_fleg[U.Gl]  *=  1) 

footprint{LEGl][axis]  =  nm-footlcoordfensl; 
if  (nm.leg^cootect_fleg[l.EG21  =1) 

footpriiit[LEG2][exisl  =  nm.foot_2_coord[exisj; 
if  (nm.leg_cootect_fUgfLEG3]  =  1) 

footpriirt(LEG3][exisl  =  nm.foot_3_coord[ixisl; 
if  (nnLleg_contect_fleg[LKG4]  =  1) 

footprint(LEG4][axis]  =  nm.foot_4_coord{exis]; 
if(nm.icg_cofitect_flegtLEG5]  ==  1) 

footptint[LEGS][axis]  =  nni.foot_5_coonl[axisj; 
if  (nnLleg_contect_fleg[LEGd]  =  1) 

footpfint(LEG6][axis]  =  nm.foot_6_ooord[exis]; 

} 

} 


/*  Joint  angle  initialize  */ 
for  (leg“LEGl;  leg<=LEG6;  legt-*-)  { 
for(joint=CB;joint<*FOOT;joint++)  { 
jangIeneg]Doiot]  =  nm.inbd Jowt_aaglePe^[}oiiit]; 

oidiangleiieg][joint]  =jangie[leg]Ooint];  _ 

//Tricky  pert!  inbdjoint  angle0[HIP->KNEE2](HIP,KNl,KN2,FT} 
//Tridcy  part!  jangTentCB  .>KNEE2HCB.HIP,KN1,KN2.FT} 


} 

} 


dbm  >  vp.bodyspeed; 


/*  to  increment  body  position  •/ 
bodypCW]  +=  dbm*cos(wp.direction*DR); 
body[YW]  +=  dbm*sin(wp.diiection*DR); 
bodylZW]  +=  0.0; 


/•to  set  next  status*/ 
nni.body_center_coord[3]  *  bodypCW]; 
nm.body_center_cootd[4j  =  bodylYW]; 
nm.body_center_ooord[5]  =  body{ZW]; 

/•  to  calculate  change  in  body  position  and  euler  angles  •/ 
for  (axis»=XW;  axis<=ZW;  axis-*-*-)  { 

deuler[axis]  =  euler[axis]  -  oldeuler[axis]; 
dbody[axis]  =  body[axisi  -  oldbody[axisl; 

} 

/*  to  calculate  diange  in  BODY  position  */ 
nm.bodymotion[0]  =  deuler[AZIMUTH]; 
nm.bodyinotion[l]  =  deuler{ELEVAT10N]; 
nm.bodymotion(2]  =  deuler[ROLL]; 
nni.bodymotion[3]  =  dbody[XW]; 
nm.bo(fyniotion[4]  -  dbody[YW]; 
nin.bodymotion[5j  =  dbodyizWJ; 


/•  to  calculate  joint  angles  •/ 

for  (Ieg=LEGl;  leg<=LEGd;  leg*-+)  { 


if(((wp.phue-=0)ft&((leg%2)’»0))  //^  ANDo<Jd  teg# 

K(wp.phue"  I)A&((leg%2)>~  ]))){  //tpl  AND  even  le^ 

/*  compute  FOOT  poshion  on  «n  ellipie  curve  respect  to  WORLD  */ 
eUipsc(wp,  footprint{Ieg],  jpoint[leg][FOOT]); 

/*  Foot  Contact  Caiiditioa*///TeiTaia  Model  =^rUt 
if  (jpoiiltaeg][FOOT][ZWl  >  0.0)  { 

jpoint(leg][FOOT]pcW]  =  foo^print[leg]p(W] 
wp.stride*oos(wp.direction*DR); 
jpoirtfled[FOOTl[YW] »  fooqjrinl(legl[YW] 

+  wp.sttide*sin(wp.dtrectian*DR); 

jpoint(leg][FOOT](ZW]>footpriiit[Ieg][ZW];  //O.O; 

} 

} 

/*  Coordinates  Tiansfonnatioo(BODY  -tVORLD)*/ 
body_wortd(euler,  body,  jpoiid[Ieg][FOOT|); 

/*  Inverse  Kinematics  */ 

inv_laneinatics()point[]eg][FOOT],  janglepeg]); 

} 

/*  set  joint  angle  */ 

fi)r(joint=CB;joint<”KNEE2;joiiit++)  { 

for  (leg^LEGl;  leg<-LEGd;  leg^-^)  { 
nm.iuMJoint_anglePe^(joint]  jangle[leg][joiiit]; 

//Tricky  part!  inbdJoint_angIeDIHn>->KNEE2]  {HIP,KN1,KN2.FT> 
//Trickvpart!  jangleQ[CB  ->KNEE21{CB,HIP.KN1.KN2,FT} 

} 

} 

/*  to  calculate  change  in  joint  angles  */ 
for  (joint=HIP;  joint<=KNEE2;  joint++)  { 

nm.leglinotioa[;oint>l]  >=  jangleILEGl]{)oint]  •  oldjanglelLEGlJIjoint]; 
nin.Ieg2niotion[ioint-l]  ^  jangle(LEG2][joint]  •  oidjangle(LEG2][joint]; 
nm.leg3motion[joiiit-l]  =jangle(LEG3][joint]  •oldjangle[LEG3)[joiiit]; 
nm.leg4inotioo[ioint-l]  =  jangie[LEG4][joint]  •  oldjangle[LEG4]|joint]i 
nm.legSmotion{ioint>l]  ■‘jangle(LEGSj[joint]  •oIdjangle[LEG3][joint]; 
nin.Ieg6niatioa[ioint-l]  ^  jangIe{LEGd][iomt]  •  oldjangIe[LEC6]Qoint); 

} 

}/•  End  of  cont_tripod_inotion  •/ 

//*«***••»*•*«*«*••«*»••**.**«***•*«•«*•«««•**•••««***««*«••*««***•**•» 

//  FUNCTION:  ctp_foot0  (Center  of  the  Tripod  ->  FOOT) 

void  ctp_fi)ot(inttp_no.  double  ctpQ,  double  footOpCVZ],  double  tripodsize) 

{ 

double  ratio  =  3.0/2.0; 
intleg; 

for  (leg“LEGl;  leg<=LEG6;  leg^-+)  { 

if  (  ((tp_no  =  0)  &&  (Geg%2)  =  0)) « 

((^_no  =  1)  &&  ((leg%2)  =!))){ 
foot[leg]pCW]  =  ctppCW]  +  tripodsize*cos(ieg*60.0*DR); 
foot[leg][YW]  =  ctp[YW]  +  tripodsize*sin(leg*60.0*DR); 
footpeg](ZW]  =  ctp(ZW]; 

} 

} 

}//  End  of  ctp_foot0 

//  FUNCTION:  foot_ctp0  (FOOT  ->  Center  of  the  Tripod! 
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voidfoot_clp(ii<tp  no,  doubk  c4>[].  doubk  footQpCYZ]) 

{ 


ctppq  “  (fbot[^_iK>]pC]  +  fbo»ltp_i»+2]pCl  +  fi)o»(tp_iio+4][X]y3.0; 
ctp[Y]  =  (foo»(tp_no]pr]  +  fbol{tp_iio+2]m  +  £boH^_i»<4J[Y]y3.0; 
ct|i[Z]  =  (foot[tp_iio][Z]  +  fi>at(tp_D»4-2][Z]  +  foot[tp_iio+4](Z])0.0; 

}//  End  <rf'  foot_ctpO 
//  TITLE:  pniit_tfatusO 

//FUNCTION:  tc ptint out Nait_iiicitioa das vanable status 
//INPUTS:  class  Next  Molkn, 

//OUTPUTS:  None. 

//  CALLED  BY:  MotionPUnoingO. 

//CALLS:  None 

void  print  status(Next  Motion  Anm) 

{ 

intleg; 

printfOn— AQUAROBOT  CURRENT  STATUS— \n'); 
priiitflrPOSrnON\n"); 

printfl:”  BODY  Xw==?tf  Yw-«tfZw==%I\n",  nm.body_center_coofd{3]. 
nm.body_center_coord[4], 
ontbody  oeater_coord[S]); 

prints  FOOTl  Xw=4tf  Yw=9<i‘Zw=%fti",  nnLfoot_l_eoofdpoV], 
nmibot_l_coofd(YW], 
nmibot  1  coord[ZW]); 

prinlfr'  FOOT2  Xw»?tf  Yw-WZ»r=%fti",  iimjfoot_2_coofdp{W], 
nmibot_2_ooord(YW], 
nmibot  2_ooord(2W]); 

printfl’  FOOT3  Xw=%f  Yw=WZw=?«\n",  nmibot_3_coo»dp<W], 
iiinibot_3_ooord{YW], 
nuLfoot  3  oootd(ZW]); 

printfC  FOOT4Xw»^tfYw-WZw-?«\B^nn^Toot_4_coofdpfW], 
nnLfoot_4_oootd[YW], 
nmibot  4  ooocd[ZW]); 

printir  FOOTS  Xw=%fYw-WZw-%I\n”,nmibot_5_coooipCW], 
nnLfoot_5_cootdpn^, 
nmfoot  S  ooo(d[ZW]); 

printir  FOOTS  Xw=Stf  Y\v=WZMr-%l\n",  iimfoot_6_coordpcW], 
nm.foot_6_coord[YW], 
iimfoot_6_coordlZW]); 

printir  JOINT  ANGLE  \n"  ); 
fof^eg=LEGl;  leg<=LEGS;  leg++){ 
priitir  LEG*/4dHIP=StfKl=%fK2=%Fn",leg+l. 

nm.inbdJoiiit_angle[leg][HIP],  //HIP 
iminbdJoint_angle[leg](KNEEl],  //KNEEl 
nm.inbdJoiiit_angle[leg]pOIEE2])'//KN£E2 

> 

prinUTFOOT  CONTACT  FLAG  'n’); 
for(leg*LEGl;  leg<=LEG6;  leg+-t-){ 

printir  le£*Ad=^",  teg+l,nmleg_ooiitact_ilag(leg]X 

} 

printir'”"); 

}//  End  of  piint_statusO 

//•**«*•***•*•••«•••••«•**•*«•••*•*•••«*•••••**••**•••«•.***•*•*. 

//  TITLE:  priiit_walkpanO 

//FUNCTION:  to  print  out  WalkParameler  class  variables  status 
//INPUTS:  class  Walkparameter. 

//OUTPUTS:  None. 

//  CALLED  BY :  MotionPlanningO- 


//CALLS:  Nooe 

//•••••••*•••••••••••*••••••••••••••••••••••••••«*•••••«»•••••••••••••• 

void  print  wmlkpara(WalkPaniiieler  ftwp) 

{ 

prindOn— WALK  PARAMETERS— \o''); 
prii)tfl[*PHASE~%(l\n*,wp.piiase); 

prinlirSTRlDE>%f  DIR£CnON^A«a”.wp.stride.  wp.difecti<»); 
prinri(’TPSIZE«^FOTHEIGHT>*%^ii*,vvp.tripodsize,  wp.footheiglit); 
printf(*BODY  SPEEl>*f^',vvp.body3pecd); 

}//  End  of  priid_walkpanO 
//  END  OF  FILE  "aitpgaitC" 

// FUNCTION:  print_jDU  daUQ 

void  priiit_gnu_dria(Next_Matiaa  Anm) 

{ 

dmible  £-jler[3];  //  Euler  angle 

double  body[3];  //  Body  Positiao 

double  jangle[LEG][JOINTl>  H  angle 
double  jpoint[LEG][JOINT]pCYZ];  //  Joint  Poeition 
int  leg,  joint; 

/*  print  for  GNU  plot  data  file  */ 

FILE  •IpO.  *^>1.  *^2; 

^  =  fbpcnCaitpl.datVa"); 

4>1  =  fopenCe«y.dat',"w"); 
fi>2  °  fopeo(’afstable.dat’,‘w”); 

/*  Initialization  */ 

eulerfO]  *  nm.body_center_coord[0]; 
euletfl]  *  nm.body_center_ooord[l]; 
euler(2]  =  nm.body_center_ooord[2]; 
bodypGVj  =  mn.body__(xater_coor^]; 
body(YAV]  =•  nm.body_oenter_coard[4]; 
bodyfZW]  =  nfii.body_center_Goord[3]; 

/*  joint  angle  initialize  */ 
for  Oeg-LEGl;  leg<=LEG6;  le^)  { 
for(joinb=CB;joint<=FOOT;joint++)  { 
jangle[Ieg][)oint]  -  nm.inbdJoinl_anglePeg][joint]; 

} 

} 

for  (leg=LEGl;  leg<-LEG6;  leg++)  { 

/*  Kinemafics  computation  for  GNU  plot  data  file  */ 
lcinematics(iangle[Ieg],  jpoint[leg][HIP],  jpoint[leg][KNEEl], 

jpoiii^eg][KNEE2],  jpoi^eg][FOOT]); 

fof(joint=HIP;  joint<=FOOT;  joint++){ 


/*  Coordinate  Transforination(WORLD<-BODY)  */ 
wofid_body(euler,  body,  jpoint[leg][joint]); 

Q>rintfi[ipO,''%f  ^\n"Jpoint[leg][joint]pC]Jpoint[leg][joint][Z]); 

} 

ft)rijitftlpO,"\n"); 

} 

lprintl(fpl,'^/ofyrf\n"jpoint[LEGl][FOOT]pCW]opoiiit[LEGl][FOOT][YW]); 

fi«ntf(fi»l,’«^f%f\n"Jpoiiit(LEGlJ{HIP]pCW]jpoirt[LEGl][HIPJIYW3); 

flprintl($l,“Vrf%f\n"jpoiiit[LEG2]IHIP][XW]opoiiit[LEG2][HIP][YW]); 

%intf(^l,"\n"); 

fi?riiitf(fpl,'^/tf%f\n"jpointtLEG2][FOOT][XW]jpoint[LEG2][FOOT][YW]); 

fpriiitf($l,'^/of%f\n"jpointlLEG2][HIP]pCW]jpoint[LEG2](HIP][YW]); 
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4irii<f(ft)l,'^Sf\B"jiK>ii<lIJG31[FC«TlpCWljpoirtILEG31[F^^ 

fl)riiifl:^l.'^%f\n"jpoirt[LEG3][HIP]pCW]jpomtpiG3][HIP][YW]>, 

ft)rii<<(4il.'^Sf\n"jiK)ii«(LECM)(HIP][XW]Jpoii<(U^ 

^)rirt^^l,'^Sf\n’jpoBtflLECMl[FOOTlpCWljp<>Mt(IXG4in;OOTl[Y^ 

^)fii<«^)l,'^Sf\i»"Jpo«<P£G4J(HniP£Wljpo«<(LE04J^^ 

ft)rirtfl:tl.’^%f\n"jpoW{IJEG5][FOOT|pCWj4pointIlJEG31[FOOT][Y^ 

fi)rii<«ftil.'^?tf\n"jpoiiit(LEGS](HIP]pCW]jpoii<(IiG5]n^ 

ftcinl^4>l.'^’4f'“"jP«*>*P'EG«][HIPilXWljpoii<(U»<][H^ 

^)rirtl(i5>l,'^3tf\n"jpoiBtpXG6][FOOT](XW]jpoiitfp£C56inX)OT][YW]); 

lpriiill(fpl,'^?tf'n"jpoiirttIXGl][HIP]pCWljp<)iiitI^ 

?4f  \n",  bodyfXW],  body[YW]); 

ft>rintf(^2."Sf  ?tf  \n"4pon<n^Un'<>3TlPW]jpoiiit(IiGll[FOOT][^ 
lpriim(^)2,"3tf  \n”jpoiiit{IXG3]IFOOTIpCW]jpoiiit(LEG3](FOOT][YW]); 

W\n"jpoiirt(IiG5](FOOT|pCW]jpoiiit(LEG51[FOOT)[YW]); 
fpmeSifyl.’Vt  ^n"jpoilltiI^l]IFOOTlpOA^  jpoiiit(IiGl][FOOT]nrW]); 
4iniit^4i2,na‘'); 

^irii!tf(^2,"3tf  \n"j|)onitCLEG2][FOOT][XW)jp<>iiit(LEG2)(FOOTl[YW]); 
4)riiilfl:^2.-%f  Sf\n"jpoint(l£G41[K)OT]p{W]jp<)aitILEG4][FOOT][Y^ 
§>riiitf(i5)2,*%f  \n"jiK)bit(lXG6][FOOTlpCW]jpoirtILEG«l(FOOTlpnV]); 
fpiitff(lp2,"3tf  W\n" jpoinlIl£G2](FOOTlpCW] jpointILEG2][FOOT][YW]); 

ftirintfl:^2,"3tf  \n",  bodypCW],  bodyfYWJ); 
fimilfl:fp2,"\ii*); 

fcloM(ipO); 

iclose(4>l^ 

fcloM(4>2); 

}/•  End  of  priiit_2nu_data0  •/ 

n 

//  FUNCTION:  print Joint_daUO 

void  print  joint  data(Next  Morion  &nni) 

{ 

/*  print  for  GNU  plot  data  file  */ 

FILE  •fpll,  *^21,  *^31,  *^1.  *4)51.  ‘Ipei; 

FILE  *^12,  *^22,  *^32,  •^>42,  •^52,  *'r«2; 

FILE  *fyI3,  •lp23,  *^33.  •§>43,  •§>5:  .  ^i^i\ 

§>I1  =  fopen("legIhip.dat",“a"); 

§)12  =  fopenCleglknLdatV*"); 

§>13  =  fopeoClegll(ii2.dalVa"); 

§>2I  =  fi)peoneg21np.<Ial’ "a"); 

$22  =  fopenneg2knl.datVa"); 

§>23  =  fopeiineg2kn2.dal","»");  . 

§>31  =fopen(neg3hip.dat-,"a"); 

§)32  =  fopeii(Teg3knl.dal”."a"); 

§>33  =  fbpenneg3kn2.dalVa"); 

$41  =  fopenneg41iip.dat","a"); 

§>42  =  fopenCleg4knl.dal","a7, 

$43  -  fopenneg4kn2.datV»"); 

$51  =■  fopenOegShip-datVa"); 

§)52  =  fopeiineg5tail.dat","a"); 

§>53  »  fopenCleg5kn2.dal","a"); 

§)61 «  fi)penneg6hip.dat","a"); 

§>62  =  fopeiirieg6knl.dalV«"); 

§>63  »  fopeoneg6kn2.datVa"); 
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//  print  joiatuigle 

4itiiili(^ll.‘'%f\a*,nn.Uid Joinl_aiigle(LEGl][HIP]); 

4>rintf(4>21.*Sf>ii',  nnLiididJaint_aiig|e{L£G2](HIP]X 
4Kinlfl[4>31,*Sf  l■n.illbdJoi^t_■ngleiL£G3j[HIP]); 

4irin(^^l,*^tf  ^n^  nm.inbdJoiiit_iiigle[LEG4j[HIP]); 
4irinlfl[^Sl.'%f\n”.nni.inbd Jomt_an^e{LEG3](HIP]); 

^rinti(4)61,*%f\o*,  mtiiibd Joint_uigle[L£G6jlHIP]); 

\d”,  nm  iiihdJoint_«ngle[LEGl][KNEEl]>, 

4)rinUS[4>22.*%f  \n',  nm  inhd_jomt_«ng|e[LEG2][KNEEl]); 

4)riiitfl[^2,*3tf  \a",  nmirint JoinI_angle(L£G3j(KNEEli^ 

4)iiiit^i^2,''3tf  \n'',  nm  iiiM_joint_Mgle[LEG4][KNEEl]); 

4printf(l^32,''3tf  \n''.  nnLinbd_joiiit_uigte[LEGS][KNEEl]); 

4ptiiili(^2,'^\n'*,  mn.inbdJoint_aiigle[LEG6j[KNE£l]); 

%intf(^13,'^\n”.  imi.iiibdJotiit_«iigle(LEGl][KNE£2]); 

nnLinbdJoiiit_iiigle[LEG2](KNEE2]); 

4)nnlf|[4>33,”3tf  \n’,  nm.iiibdJoiot_angle[LEG3j[KNEE2]); 
fpniitf(!^3,'^\a",  iim.iiibdJomt_aiig|e[LEG4][KNEE2]); 

4)cinlfl[l^33,”%f  \n*,  iim.iiibdJoint_in^[L£GS][KNEE2]}; 

mn.tnbdJoint_ing|e[LEG6][KNEE2]); 

fcIoie(4>ll}:  fi;losc(^12);  fcla*e(4>13X 
fclote(fp21)-.  fcl<we(fp22);  fclo*e(ip23); 
fclose(4>31);  fcl<Me(1^32)-,  fcloM(]^3); 
fclace(4>41^,  fi:IoM(fi>42);  fctoce(4>43); 
fclose(4>31);  fclase(fp32);  fclose(fp33); 
fclose(fp61);  fda«e(4>62);  fclose(fp63); 

)/*  End  of  printJoiiit_dataO  */ 

#ifiidcf  ARTPGAIT  H 
#<fcfineARTPGArr_H 

FILENAME;  ar^igutH 

«««*«««***«*.*«».**M««*****«******«*««**«***«*»*««***«««****a»***»*»/ 

^include  "Kineimtics.H" 
extent 

void  get_goal(double  Q); 
extent 

int  get_menu(int); 
extent 

int  get_footcboice(iat); 
extent 

void  gait_algorithm(Nex(_Motioa  A,  double  Q,  int,  int); 
extent 

void  robot_model(Next_Motioa  &)', 
extent 

void  disc_gait_planning(Next_Motion  &,  WelkPanmeler  &,  Flag  &,  double  Q,  int); 
extent 

void  disc_set_flag(Next_Motion  &,  WalkPannteter  &,  double  Q,  Flag  &); 
extent 

void  disc_tripod_motioit(Next_Motion  &,  WallcParanteter  &,  Flag  &X 
extent 

void  cont_gait_planning(Next_Motion  &,  WalkParanteter  &,  Flag  A,  double  Q); 
extern 
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void  ooat_ict_fla((N«xt_Kloliaa  ft,  WalkPanunelcr  ft,  double  Q,  FUg  ft); 
exteni 

void  ocnt_1ripod_iiiatioa(N«a_MotM»  ft,  WeUcPannMter  ft.  Flag  ft); 
extem 

void  cl|>_fbot(tnl,  double  Q,  double  [][XYZ],  doubleX 
extent 

void  fbot_clp(int,  double  Q,  double  Q[XYZ]); 
extent 

void  iinnt_itatiu(Next_Mo(ioa  ft); 
extent 

void  priiit_waIlcpata(Wa)kPannieter  ft); 
extent 

void  print _gnu_data(Next_Moti«ti  ftX 
extent 

void  print  Joint_dita(Next_Motion  ft); 

#endif 

//FILENAME:  boLC 

//PURPOSE:  This  file  makes  a  iticfc  atpiaiobot  paphics 
//  interactive  design 

//  It  utilizes  Kinematic  fimctions  to  determine  xyz 

//  ooonlimtrs 

//  CONTAINS:  fimctiacis  shown  in  both 

//  NOTE:  This  is  aitd  DUS  3D  pro^am  written  in  C-t-v 

//  UPDATE:  Last  Update  1993  Apr  30  by  Kenji  Suzuki 

#include  "gl-h”  // graphics  library 
#ittclude  ”devioe.h”  //  graphics  library  file 

dittclude  <stdioJi>  // C-t-v  library 

#ittclude  ’botH"  //  declaration  file 
trinclude'Link.H'' 

#inciude  "AbRigid.H" 

#include  ’MatrixMy.H' 

#irtchide  ’AbBody.H’ 

#ittclude  ’Kinerrtatics.H'' 

#inciude  'AbLeg-H* 


/*  added  by  Kenji  Suzuki  1993  >^21  */ 

#itK;lude  'arcansLir 
#ittclude  "arfiittc.H'* 

#ittclude  'artpgaitH” 

mairtO 

{ 

// value  returned  finm  the  event  queue 
short  value; 
kxtg  maintttenu; 
long  hhitem; 

FILE*ilp; 

Up  -  Ibpenrbotdat","!^; 

double  goaipCY];  // goal  position,  WORLD  coordittates 
Next_Motion  actual,  comtnattd,  temp; 


int  meou  >  0; 
intfboisize  0; 

memi  -  get_inemi(incnu); 

get_go)a(*otl); 

get_footdioicc(foO»ize); 

//  inhUlizc  the  HUS  sydcm 
initUlizcO; 

//  Create  Pop  Up  Menus 
msinneiiu  ■*  nukethemeausO; 


// make  tbe  robot  firom  its  pieces 
AquarobotBody  aquabody. 

AquaLeg  legl(aqiiabody,0.0); 

AquaLeg  leg2(aquabody.60.0); 

AquaLeg  leg3(aquabody,120.0); 

AquaLeg  leg4(a<piabody,  1 80.0); 

AquaLeg  leg5(aquabody^40.0); 

AquaLeg  Ieg6(aipiabody300.0); 

Retuni_Coordiiiates  coord; 

Passing_Iteiiis  pass; 

Nexl_Motioa  trails; 

intans; 

aiis“0; 

pass.le9Him  ■*  9; 

coord  >  FiiidPositio0a(squabody,  tegl,  leg2,  Ieg3,  Ieg4,  legS,  leg6); 
trans  =  TfansferTo<3aii(coord,  aquabody); 

while  (TRUE)  { 

//  do  we  have  something  on  the  event  cpieue? 

if((FestO){ 


switch  (qread(&value))  { 

case  REDRAW: 
reshapeviewportO; 
break; 

case  MENUBUTTON:  //  Menu  selections 
if(value“  1)  { 

hititem  >  dopup(nuunmenu); 
prooessmenuhit(hititem); 

} 

bleak; 


caseAKEY: 

// the  gait  algoritfam  is  incorporated  in  this  case 
//  actii^  °  trans; 

// Something  wTQOg! 

//  "trans"  does  not  give  correct  values. 
//byKenji  1993/4/30,15:00 


temp  ~  actual; 


/•  gait  algoiitlim  •/ 

gait_algorithm(temp,  goal,  menu,  footsizeX 


command  =  tenq); 


/•  robot  model  •/ 
robot_model(temp); 
actual  =  temp; 
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tmif  ~  ooamiind; 


/•»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»•/ 


deJaz  >  traiii.bodyniotiaa(0]; 
ddd  » lnn*.bodyina6aa(l]; 
ddroi  ~  tniB.bodymotiao(2]; 
ddx  -  tnaf.badyniotiaa(3]; 
dely  -  tram.bodyiiiotioii(4]; 
delz  •*  lni]i.bod)aiotiao(S]; 

legl.MovelDCRmeaUl(«(]uabody,tniis.ieglmatiao(0], 

ttam.legliiiotioa[l],tniis.Icglmolioa[2]>. 

le^.MoveIiicreiiicnUl(«qu«bo<<y.<f»»g-l»g^»°°*^[01. 

tntB.leg2<iiotiao[l],trias.leg2iiiotioii(2]>. 

leg3.Moveliicmncnt«K«4M»<><^y.*f*°*-**t3incitioo[0). 

tniis.teg3iiiotioa[l],tnm.kg3aiotifla(2]); 

Ieg4.MoveIncntneaUl(aquabody,tnuis.teg4ino(ioa[0], 

tniis.teg4aiotiao(l],truB.leg4iiioti<»[2]>, 

legS.MoveIiicniiieaul(aqiiabody,tnm.legSiiia(ioa{0], 

tnns.legSiiiacian(l].tmis.legSiiiolion[2]); 

leg6.MoveIiicRmenUl(aqiubo(iy,ttaiB.leg6ino(ioii[0]. 

tnns.leg6nioliaa[l],1mis.leg6inotioo(2]); 

ooord  -  FiiidPositkxB(aquabody,legl.leg^,Ieg3,leg4,iegSJeg6); 

tnns  =  Triurfa’ToG«it(coocd,»qu«body); 

break; 

caseRKEY: 

rewind(ifl)); 

break; 

}  //  and  switcli  on  event  queue  item 
}  // endif  qtestO 


colo((BLUE);  // background  color 
clearO; 

//  turn  on  Z-buffering 
2buffer(TRUEX 


!'  clear  the  z-buffitr 
zcleaiQ; 

buildnoaiiiovingviewingmatiui(vx,vy,vz,(fi(^/&); 

drawaqua((coord.bodycX(coord.leglcX(coorltq^cX(coord.leg3cX 

(coord.  leg4cX(coocd.i^cX(coard.Ieg6c)); 


/*  tum  z-buffenng  off*/ 
zbuffer(FALSEX 

/*  diange  the  buffers ...  */ 
swapbuffersO; 


qenter(AKEY,l); 


} 
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}  //endc^maiii 


//  FUNCTION:  INTTIAUZEO 

//  •••••••••••••••••••«•••*•«••••••••*••••••«•**••••• 

void  initializeO 

{ 


//  set  up  the  peefened  aspect  ratio 
keepaspect(XMAXSCR£EN+l,YMAXSCR£EN+l); 

//  set  up  window  size 

//  p(e4)oaiti<»(700.0.1200.0.200.0.700.0^, 

II  preQwaitioaCZOO.0. 1300.0, 0.0, 900.0); 
pre^>a«ition(200.0, 7S0.0,  SOO.0. 900.0);  //  for  video  converter  screen 

// open  a  window  for  the  program 
wiDopen(*AquaRobot*); 

//make  a  title 
wiiUitle(*AquaRobot”); 

// put  the  nUS  into  double  buffer  mode 
(k^lebuffiaf); 

// configure  the  mis  (means  use  the  above  command  settings) 
gcottfigO; 


/*  queue  the  devices*/ 

qd^ce(R£DRAW); 

qdevice(AK£Y); 

qdevicefPKEY); 

qdevice(RK£Y); 

qdeviee(MENUBUTTON); 

vx  “  0.0; 
vy=  400.0; 
vz=  800.0; 

r&  =  0.0; 
rfy  »  0.0; 
rfe  *  0.0; 


} 


/*  Function  Make  the_Menus  */ 

. . . 

long  makethemenusO 

{ 

longtopmenu; 

long  cameramenu; 

/•  camera  views  •/ 
camerammi  newpupO; 
addtopup(cameramenu,"Camera  View  %t "); 
addtopup(cameramenu,"ABOV£  %x2 1  LEG5-d  VIEW  %*3"); 
addtopup(camerainenu,"LEGl  VIEW  Hx4 1 LEG4  VIEW  »/«t5"); 
addtopup(canieramemi,  ''LEG2>3  VIEW  %x9"); 

//  build  the  top  level  menu 

topmenu-  defi>up(”Roll  Off  Side  34t|  Camera  %xl  9tei|FileRead  %x6  |ResetFile  %x8|KeybdRead  %x7|Reset  %xl4|  Exit 
%x  1 3  ".cameramenu); 

// return  the  name  of  this  menu 


215 


reiiiniCtaimeau); 


} 


/••»•••*••••••••••••••••••*•••••••••••••••••••••••••••••••••••••••••••*/ 

/*  Function  Prooen  Menu  Hit  */ 

/m***9^00*0**0^*mm*9**9**—9»m*m*mm*m09*—*—9*—*^*9—09**00*****/ 

void  pracennienuhitOoiig  hhitrai) 

{ 

twitcli  Oiititem){ 

caie  CAMERA: /*  top  slide  ^menu  */ 
hteak; 

case  ABOVE:  // Graphics  Cootdinates 
vx  “  0.0  +  ifit; 
vy  - 1000.0  +1^ 
vz  “  0.0  +  rfe; 
bleak; 

caseLEGS6_VIEW:  /•  View  from  the  behind  robot  */ 
vx  “  0.0  +  rfic 
vy  -  400.0  +  rfy, 
vz  •=  -800.0  +  rfe; 

bteak; 

case  LEG1_VIEW; 

vx  =*  800.0 +  ffic; 
vy  -  400.0  + rfy, 
vz  ”  0.0  +  tfc, 
break; 

caseLEG4_VIEW: 

vx  « -800.0 +  rfic; 
vy  =  400.0  +  tfy, 
vz  “  0.0  +  ife; 
break; 

caseFILEREAD: 

qenterfPKEY.l); 

break; 

case  RESETFILE: 

<|eiitet(RKEY,l); 

break; 

caseLEG23_VIEW: 

vx  =  0.0  +  ific; 
vy  -  400.0  +  tff, 
vz  =  800.0  +  rfe; 

case  RESET: 

vx  -  0.0; 
vy  =  400.0; 
vz  =  800.0; 
break; 

case  EXIT: 

exit(0); 

break; 

}  //  End  Switch 


} 

/*  for  otqects  that  are  in  the  same  coordinate  system  but  arent  moving 
with  the  contiiMious  rotationsAranslatioiis/scaliiigs,  we  use  this 
routine... 

•/ 
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//  FUNCTION;  BUILONONMOVINGVIEWINGMATRIX 


void  buildDoanoviiigviewiiigaiatnx(float  vx41oit  vy^loal  vz, 
refic^loat  refy^loat  nb) 


//  we  mutf  call  loadunit  befote  we  get  the  prcjection 
//  and  viewing  stuff... 

loadunitO; 

// just  call  the  per^iective viewing  matrices 
projectiooandviewingmatrix(vx,vy.vz,refic,fefy,feff); 


/*  put  up  the  projection  and  viewing  matrix  */ 

//  FUNCTION:  PROJECTIONANDVIEWINGMATRIX 

void  projectionandviewingmatrix(float  vx^oat  vy^oat  vz, 
float  refic,float  refy^loat  r^) 


//  penpective  projection  30  for  the  world  coord  sys 
//  the  near  and  hr  values  are  distances  from  the  viewer 
II  to  the  near  and  ftf  clipping  planes. 

//  We  ate  at  (vx,vy,vz)  and  looking  towards 
//  the  center  point  of  the  object. 

//  (towards  (re&,refy,re&;)). 


per5pcctive(45O,1.25,NEARCUPP0<GJFARCLIPPING); 

lookat(vx,vy,vz,re£(,refy,re£c,0); 


// this  routine  loads  a  unit  matrix  onto  the  top  of  the  stack 

void  loadunitO 

{ 

static  float  un[4][4]  =  {  10, 0.0, 0.0, 0.0, 


//load  the  matrix 
loadmatrixfun); 


0.0, 1.0, 0.0, 0.0, 
0.0, 0.0, 1.0, 0.0, 
0.0, 0  0,0.0, 1.0  ); 


//  AQUA  DRAWING 
void  drawaqua(double  *bodyc. 


colotfWHITE); 

linewidth(3); 


double  *leglc,  double  *leg2c,  double  *leg3c, 
double  *leg4c,  double  *legSc,  double  *leg6c) 


//NOTE!!!!!!!!! 

//  +x  to  right,  +y  up,  +z  out  of  screen  ->for  graphics 
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//  +x  out  of  legl,  +y  out  of  screen,  -z  dowi»->for  aquerobot 

//  X  z  y 

move(bodyc{3],-bodyc(3].bo(lyc[4]X 

draw(bodyc{6],-bodi^8].bodyc[7]); 

dnw(bodyc(9].-bo<^l  l].bo<lyc[IO]X 

draw(bodyc{12],4>odyc(14],bodyc[13]); 

drew(bodyc(15),-bo<^171,bo<^16]>, 

dtaw(bodyc(18].-bodyc{20].bodyc{19]X 

tewi(badyc[3],4>odyc[S].bodyc{4]X 

// drawee  line  fixxn  body  center  to  iegl  joint  \ 

linewidlfaflX 

niove(bodyc[0],-bodyc[2],bodyc(l]X 

draw(bodycP),-bodyc(5J,bodyc(4]); 

// draws  legl 

oolorOfELLOW); 

linewidthfSX 

//  X  2  y 

move(leglc[0],-leglc[2].leglc(11); 

draw(leglc{3].-Ieglc[51Jeglc(4JX 

draw0eglc(6],-leglci8],lcglc[7]); 

draw(leglc[9j,-teglc(l  l]Jeglc[lO]); 

// draws  ieg^ 
ooMGREEN); 

Iinewidlh(SX 

inov«(le^c[0],-leg2c[2],Ieg2c(  1  ]); 
draw<Ieg2c(3].-leg2c[S].le^c(4]X 
drawaeg2c{6].-le^[8].lcg2c[7]X 
draw(Ieg2c{9].-leg2c[l  ll,leg2c[10]); 

//  draws  leg3 
coloifGREEN); 

IiDewi(ldi(5); 

move(leg3c(0],<leg3c[2]  Jeg3c(  1  ]); 
draw<Ieg3c(3],-leg3c[S]jc0c[4]X 
drsw(leg3c(6],-leg3c[8],leg3c[7]X 
draw(leg3G[9].-leg3c[l  l]Jeg3c[10]); 

//  draws  leg4 

coloifGREEN); 

tinewidUi(3X 

move(leg4c(0],-leg4c(2]Jeg4c{l]X 
(baw(Ieg4c{3],-leg4c[S],leg4c[4]}', 
draw^eg4c{6],-leg4ci8],leg4c[7]X 
draw^eg4c(9j,-ieg4c[l  l]Jeg4c[10]); 

//  draws  legS 

color(GR££N); 

iinewidlh(SX 

inove(Ie^c[0].-leg3c[2],leg3c[l]); 
draw(leg5c(3],-leg5c[5],leg5c(4]); 
draw^egSci6j,-legSc[8jjeg3c[7]); 
draw0egSc[9j,-legSc[l  l],IegSc[10]X 

//  draws  leg6 

coloifGREEN); 

liiiewidth(3); 

inove(legfo[0],-leg6c[2],leg6c[l]); 
draw(leg6cp],-leg6c[S],leg6c(4]); 
draw0eg6ci6].>leg6c[8j,leg6c{7]X 
draw<Ieg6c[9],>leg6c|l  I],leg6c[l0]); 


} 
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1 1  »<»««»»»»»> 

//  FILENAME;  boLH 

//  PURPOSE:  de&M*  OMMtann  and  fiiuctioBi  used  iot  botC 
//  NOTE:  TIus  is  an  IRIS  3D  program  written  in  C-t-*- 


#ifcdef  BOT_H 
#define_BOT_H 

//  provides  conattnls  for  menu  processing  opcions 

Mefine  CAMERA  1 

#defiiie  ABOVE  2 

#defiDeL£G56  VIEW  3 

ddeEneLEGl  ^W4 

#define  LEG4_VIEW  S 

#define  FILEREAD  6 

#de&w  RESETFILE  8 

#de6ne  LEG23_VIEW  9 

#defineR£S£T14 

Mefine  EXIT  IS 

Mefine  NEARCUPPING  10.0  //  planes  defined 
//  #define  FARCUPPING  1023.0 

#defiiie  FARCUPPING  2047.0  //  changed  14MAY93;  C.Schue 


long  maketbemenusO; 

static  float  ttcj*  reference  point  on  in  the  x  direction  */ 
static  float  tfyj*  reference  point  on  in  the  y  direction  */ 
static  float  ticj*  reference  point  on  in  the  z  doectioo  */ 

static  float  vx;  /*  view  point  on  in  the  x  direction  */ 
static  float  vy,  /*  view  point  on  in  the  y  direction  */ 
static  float  vz;  /*  view  point  on  in  the  z  direction  */ 

double  de)ul,delta2,delta3; 
double  delaz.delel,delrol.delx,dely,delz; 

void  proccssroenuhitflong  hititem); 

void  initializeO;  H  initializes  graphics  layout 

void  loadunitO;  H  a  unit  matrix  used  in  rotatioWlranslation 

void  pfojectionaiidviewingniattix(float  vx.  float  vy,  float  vz, 
float  refit,  float  refy,  float  refit); 

void  buildnonmovingviewingniatrixffloat  vx,  float  vy,  float  vz, 
float  refit,  float  refy,  float  refo); 

void  drawaquafdouble  *,  double  *,  double  *,  double  *, 

double  *,  double  *,  double  *);  //  includes  all  legs  and  body 


#eodif 

'/  niENAME:  Kinematics.C 

//PURPOSE;  to  determine  positioos(x,y,z)fipom  the  H_matrix 
//  to  read  from  a  file  the  new  link  angle  dianges 
//  and  update  the  appropiate  leg/link  values 

#include  <inath.h> 

#include  <st(Uib.h> 

#include  <stdioJi> 

^include  "MatrixMy.H" 
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#include  ’AbLeg.H* 
^include  'AbBody.H" 
tfiiichide  ’LudoH’ 
#tiidii(ie  ’LinkO.H' 
iWnchide  *KiiienMlici.H* 


^include ’arcaosLH" 


iMefine  GROUNDELEVATION  0.0 

//xyz  coordinates  are  determined  fixm  the  H_iiiatrix  and 
// return  the  coordinates  using  the  Retum_Coordinates  structure 

Retum_Coorditudes  FindPositioiis(A<|uarobotBody  &body, 

&Iegl,  AquaLeg  &le^,  AquaLeg  &le^, 

&leg4,  AquaLeg  &legS,  AquaLeg  &leg6) 

{ 

Retum_Coordinates  *rc; 
rc  =  new  Retum_Coordinates; 

// body  center  coordinates 
rc->bodyc(0]  =*  body.body_list->val(0,0); 
ro->bodyc[li  =  body.body_list->val(0,l); 
fc->bodyc[2]  =  body.body_list->val(0,2); 

// body  points  to  draw 
rc->bodyc[3]  *  body.body_list->val(l,0); 
rc->bodyc[4] »  body.body_list->val(l,l); 
rc->bodyc[5]  *body.body_Ust->val(l^); 
rc->bodyc[6]  •=  body.body_list->val(2,0); 
rc->bodyc[7]  =  body.body_list->val(2,l); 
rc->bodyc[8i  =  body.body_liit->vaI(2^); 
rc->bodyc[9]  =  body.body_list->val(3,0); 
rc->bodyc[10]  =  body.body_Ust->vaI(3,l); 
rc->bodyc(lli  «  body.body_iist->val(34); 
rc->bodyc[12]  *  body.body_list->val(4,0); 
rc->bodyc[13]  =  body.body_list->vaI(4,l); 
rc->bodyc[14i  =  body.body_list->val(4^); 
re->bodyc[15i  *»body.body_list->val(5,0); 
rc->bodyc[16]  =  body.body_list->val(5,l); 
rc->bodyc[17]  =  body.body_list->val(5,2); 
rc->bodyc[18]  =  body.body_list->val(6,0); 
rc->bodyc[19]  =  body.body_list->val(6,l); 
rc->bodyc[20j  =  body.body_list->vaI(6^); 

//  prints  out  body  coordinates 

/• 

printl(Tx)dy  center  %3f ,  %3f ,  %3f  \n"jc->bodyc[0]4c->bodyc[l], 
rc->b^cp]); 

printflTbody  pt  1  %3L  343L  %3f  \n"^>bodyc[3]^>bodyc[4], 
rc->b^c[5)); 

printt^''body  pt  2  %3€  %3L  /43f\n"jc->bodyc[6]^>bodyci7], 
rc->bodyc[8]X 

printfC"^  pt  3  %3t  %3£  %3f  \n"jc->bodyc[91^>bodyc[10], 
rc->bodyc[ll]); 

printirbody  pt  4  %3L  %3£  %3f  \n"jc->bodyc[12]jx>>bodyc(l3], 
rc->bodyc[14]); 

printf|>>dy  pt  5  •/at,  %3t  %3f  \n"^>bodyc[15]^>bodyc[16], 
ro->b^c(17J); 

piintf("bo^  pt  6  %3L  %3C  %3f\n>n"^>bodyc[l8]jc->bodyc[19], 
rc->b^c[20]); 

•/ 


AquaLeg 

Aqualjeg 


//joint  one  teg  coordinates:  [0]=x  [l]°°y  [2]=n. 
rc->leglc[0]  =  legl.linkO->H_niatrix->vJ(0,3); 
rc->IegJJc[0]  =  leg^.link0->H_matrix->val(0,3); 
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rc->lcg3c[0] » le^.link0->H_miliix->vil(03); 
ro->lq4c[0] » leg4.1inlcO->H_in«lrix->wl(03X 
re-»egSc[0]  -  teg5.link0->H_inatrix<>vtl(03); 
n»l«g6c[0]  *  leg6.1iiikO->H_initrix->vi](0^); 
i«->leglc[l]  -  legl.UnkO->H_iiMtrix->v»l(l^); 
i«->ieg;2c[l  j  -  leg2.liiikO->H_i>Mtrix->vi](14); 
rc->leg3c[l]  >  teg3.liidcO->H  iiutfrix->vi^l3); 
rc->leg4c{l]  -  leg4.1iiikO->Hlinatrix.>val(U); 
io->legSc[l]  •  Ieg5.1inkO->H_iD»ttix->vil(13); 
i«->leg6c[l] » leg6.1iiik.0->H_in*trix->val(13); 

ic->leglc[2]  -  legl.linkO->H_iMlrix->v«l(23); 
fc->leg2c[2]  =  leg2.1inlcO->H_iMJrix->vil(2,3); 
rc->leg3c{2]  =  le^.liiik(J->H_mitrix->v*l(23); 
rc->leg4c(2]  =  leg4.liiikO->H_inalrix->viI^^); 
rc->leg3ci2]  *  leg5.1inlcO->H_iB»lrix->vil(23); 
fc->teg6c[2]  =  leg6.1iiikO->H_mitrix->v»l(2^X. 

//  inboard  joint  angle  info.  HIP  Joint  angle 

//  DD  *  Pegllioi”*] 

rc->inbd_ioint_angle[0][l]  =  legl.tinkO-><jetInboardJotatAiigleO 
rG->inbdJoint_angle[l][l]  “  leg2.linkO-><]etIiiboanUoiiitAng|eO 
rc->inbdJoint_angle[2][l]  leg3.1ink.0->GetlnboanUoiotAngle0 
rc->inbdJoiiit_ang|e(3][l]  =  leg4.1inkO->GetInboardJointAngleO 
rc->inbdJoint_ang|e[4](l]  =  legS.linkO->GetInboardJointAngleO 
rc->inbdJoint_angle[3][l]  =  leg6.linkO->GetInboanUointAngleO 

//joint  2  x,y^  coordinates  (3]=x  (4]=y  (5J«z 
ro->leglc[3]  =  legI.linlcl->H_inatrix->^04); 
rc->teg2cP]  =  ieg2.linkl->H_inatrix->val(03); 
ic->leg3cP]  =  ieg3.1inkI->H_inatiix->val(0^); 
ro->ieg4cP]  =  leg4.linkl->H_o>atrix->val(0^); 
io->leg3cp]  =  legS.linkl'>H  otalrix->val(0^); 
fc->leg6cp]  =  leg6.linIcl->Hlinatiix->val(0^); 
ro->leglc{4]  ■=  legl.linkl->H  malr«->val(l^); 
fc->leg2c(4] » Ieg2.1inkl->H jnat)rot->val(U); 
fc->leg3c[4]  *  leg3.litdcl*>H_inatrix>>val(14)'. 
rc->ieg4c[4) » leg4.liiilcl.>H_matrix->val(l^); 
rc->leg5c(4]  =  tegS.linkl->H_inatruc->val(l^); 
rc'>teg6c[4]  *  leg6.linkl->H_inatrix->val(13); 

rc->leglc[S]  =  legl.linkl->H_ioatrix->vil(23); 
iiDO>i^c(5j  =  leg2.linkl->H_matrix->val(2^); 
rc->leg3c(5]  =  leg3.linlcl->H_matrix->val(23); 
tc->leg4cP]  *  Ieg4.linkl->H_inaIrix->vaJ(2^); 
rc->leg5c(5]  =  leg5.liidcl->H_inatrix->val(2^); 
rc->leg6c[S]  =  leg6.linlcl->H_matrix->val(2^); 

//  joint  2  niQlioa_liinit_flag 

rc->niotion_limit_ilag[0]  -  iegl.linicl->GetMotionLiniitFlagO'. 
rc->motion_iinut_fUgP]  =  tegZ.Iinkl->GetMotionUmitFlagO; 
rc->motioa_]imit_flag[6]  “  leg3.1inkI->GetMotionIjniitFlagO: 
tc->motionJiniit_flag[9]  =  leg4.linlcl-X3etMotiooLiinitFlag0‘. 
ic->inotion_liniit_flag{12]  =  iegS.linlcl->GetMotionLiinitF1agO; 
rc->n)otion_limit_ilag[lS]  =  leg6.1inkl*>GetMotionLiniitFlagO; 


//  inboard  joint  angle  info.  KNEEl  joint  angle 
//  DD  =  PegjOoint] 

rc->inbdjoint_angle[0][2]  -  legl.linkl->GetInboardJointAngleO 
rc->inbdJoint_angle[l]p]  =  leg2.1inlcl-><jetInboardJointAngleO 
rc->inbdJoint_angle[2][2]  =  leg3.linkl->GetInboardIointAngleO 
rc->inbdJoiiit_anglep][2]  =  leg4.1inkl->GetInboardJointAngleO 
rc*>inbdJoint_angle[4][2]  =  legS.linkl-XjetlnboardJointAngleO 
n>>inbdJoiiit_angle[5][2]  =  leg6.linkl->GetInboardJointAngleO 

//joint  3  xyz  coordinates  [6]*x  (7]=y  [8]=z 
rc->leglc[6]  =  legl.link2->H_niatrix->^(03); 
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rc-^et^6]  •  leg3-l“>k2'^H_ni*trix->vil(0^); 
rc-»eg3c{6]  -  Ieg3.1ink2->H  iiiatrix->val(0^); 
ic->leg<c(6]  •  leg4.Uiik2o^H_BMlrix->val(03); 
ic->l45c(6j  •  ieg5.1^->H_iiutnx->vaI(0^^> 
ro^leg6c[6]  -  leg6.Iiiik2->H_milrix->v«l(0^); 

rc->kglcr7]  -  Iegl.liiik2->H  iMlrix->v«l(UX 
re->teg2c{71  -  leg2.1ink2->H_iii»trix->v«l(U>. 
rc->leg3c[7]  =•  l«g3.1iiik2->H_iiBtfrix->v»l(U); 
to->le^[7]  =  leg4.1iiik2->H_inatrix->vil(14); 
n>>lcg3c[7] » tegS.liiik2->H  m«truc->v»l(l^); 
ro->leg6c(7]  =  leg6.!iiik2->Hlmalrix->val(I^); 

ro->leglc[8]  =  legl.yiik2->H_in*ttix->val(2^); 
ro->leg2c[81  >=  leg2.1ink2->H_mittix->vil(2^); 
ro->leg3c[8]  “  leg3.1ink2->H_m*lrix->val(24); 
rc->Ieg4c[8]  =  leg4.1iiik2*>H_iiiatrix->vaI(2^); 
rc->leg3c[8]  =  leg5.1ink2->H_iii«lrix->v»I(24); 
ic->leg6c[8]  =  leg6.1iiik2->H_iiiilrix->v»l(23); 

//  joint  3  motioa_liniit_flag 

rc->iiiotion_limit_flag[l]  “  legl.Unk2->G«tMotiooLiinitFUgO; 
rc->inolion_liniit_flng[4]  *  leg^.link2*^GctMotiooLiniitFlngO; 
to->iiK)tioa_limftjQag[7]  =  legS-link^-^GetMotiooLmiitFlngOi 
rc->iiiotioa_liinit_fUg(10]  *  leg4.1ink2->GetMotionLi(nitFUgO; 
rc->matioa_limit_flag(13]  ”  leg5.1ink2->0€tMotiooLiinitFI»g(); 
rc-^motion  limit_fUg(16]  leg6.link2->GetMotionLimitFUg(); 

//  inbonrd  joint  angle  info.  KNEE2  joint  angle 

//  nO”P«8l[i®^] 

rc->iidxijoint_ang]e[0][3]  ”  legl.link2*>G*tInboaniJointAngleO 
rc->inbd_Joint  an^jljP]  ^  leg2.1ink2->GctInboanlJointAngieO 
rc->inM_joint  angte[2][3]  Ieg3.1ink2->GctInboardJointAngleO 
rc->iftbdJoint~ang|e[3][3] » leg4.1ink2->CetInboardJointAngleO 
rc->inb<l_joint_ang|e[4][3]  *  leg5.liiic2->GetInboaidIointAngleO 
ro->iobdJaint~aqglef5jpj  “  kg6.1iiik2o>GdInboardJointAligle0 

//joint  4  xyz  coordinates  [9]®*  tlO]“y  (1 1]“2 
rc->teglc(9] » legl.l'tik3->H_niatrix->vaK0^); 
rc->le^cf9]  =  le^.link3->H_inalrix->val(0^); 
rc->teg3c[9]  *  leg3.1ink3->H_niatrix->VBl(0^); 
fc->leg4c[9]  “  leg4.1ink3->H_inatiix->val(0^); 
rc->leg3c[9]  =  leg3.1ink3->H_matrix->val(03); 
rc->leg6c[9j  =  leg6.1ink3->H_niatrix->val(0^); 

rc->leglc[10]  •  legl.link3->H_niatrix->val(l,3); 
rc->lej{2c[10]  =  le^.link3*>H_niatrix->vil(l^); 
rc->leg3c(10]  =  leg3.1ink3->H_niatrix->val(U); 
rc->leg4c[10]  *  leg4.1ink3->H_matiix->vaI(l^); 
ro->legSc[10]  =  leg5.1ink3'>H_matiix->val(l^); 
rc->Ieg6c[10]  =  leg6.liids3->H_niatiix->val(l^); 

rc->leglc[ll]  =  legl.link3.>H_inatrix->vaI(2^); 
rc->leg2c[ll]  =  Ieg2.link3->H_matiix->vaI(2^); 
rc->leg3c[ll]  =  leg3.link3->H_niatrix->val(2^); 
rc->leg4c[l  1]  =  leg4,link3->H_niatrix->vil(24); 
rc->legSc[ll]  « leg5.1ink3->H_niatrix->vil(2^); 
rc->Ieg6c[ll]  =  leg!6.1ink3'>H_matrix->vil(2^); 

//joint  4  motion_Iimit_flag 

rc->niotion_limit_flag[2]  “  legl.link3->CjetMotionLiniitFlagO; 
rc*>nxitioa_Iinut_fiag[S]  =  le{^.link3'^OetMolionLiniitFlagO; 
rc->niotion_liniit_fIag[8]  =  leg3.1ink3->C5«tMotionLunitFlagO; 
rc->rootion_limit_flag[ll]  =  leg4.1ink3->C5etMotionLiniitFlagQ; 
rc->motioo_lin)itjQag(14]  =  leg5.1ink3*>GetMotionLimitFlagO; 
rc->niotion_lin>it_llag[17]  =  leg6.Unk3->GetMotionLimitFIagO; 
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//  inboaid  joiot  logic  info.  FOOT  joiot  angle 

II  DD  " 

rc->inM_)oiHt_angle[0][4]  •  legl.link3->GctInboardJointAn^eO; 
ic->inbdJoiat_angte[l][4]  •  leg?.link3->GctInb(MnlJoiatAngleO; 
rc->inbdJoia(_angle[2][4]  ~  leg3.link3->GetInboardlointAo^eO; 
ro->inbdjoint_iog|e[3][4]  -  lcg4.linlc3->GelInboanUointAngleO-, 
ic->tnbdJoint_angle[4][4]  -  leg5.linlc3->GdInhoardJointAn^eO', 
ro->tnbdjoiiit_angle[3][4]  -  legiS.link3*>GetInboardJointAngleO; 

//  teat  fof  mpporting  legi  and  idjialing  lcg;_Mpport_flig 
if  (fobi(ic->teglc[ll])  GROUNDELEVATION)  legl.SelLegSu]ipoftFUg(l)-, 

else  legl-Setl^egSupportFIagfO); 

if  (&l)e(n!->leg2c[n])  >>  GROUNDELEVATION)  leg2.SetLegSuppoctFlag(l); 
elM  le^.SetLegSupporlFlag(0); 

if  (fia)a(re->leg3c[U])  >“  GROUNDELEVATION)  leg3.SetUgSupp«tFlag(l); 
else  leg3.Se(LegSupportFUg(0); 

if  (fabi(fO->leg4c[ll])  >-  GROUNDELEVATION)  leg4.SetLegSuppoftFlag(l); 
else  le^.SetLegSuppoftFlag(0^ 

if  (fobs(re->legSc[l  1])  >~  GROUNDELEVATION)  leg3.SetLegSuppoftFlag(l); 
else  leg3.SetLegStq)pOftFlag(0); 

if(&bs(n»leg6c[ll])  GROUNDELEVATION)  leg6.SetLegSuppoflFlag(l); 
else  le^.SetLegSuppoftFIagfOX 

//  places  leg_suppo(t_fUg  into  rc 
rc->ieg_support_flag(0]  °  legl.GetLegSuppartFlagO: 
rc->leg;_supj>oft_flag[l]  -  leg2.GetLegSiq)poftFlagO; 
ro->leg;_suppart_flag[2]  =  legJ.GetLegSupportFlagO; 
rc->leg;_sitppott_ilag(3]  =  leg4.GetLegSiipportFlagO; 
rc->leg;_suppaft_ilag[4]  •  legS.GetLegSupportFIagO; 
rc>>leg^suppoft_flag[3]  <•  legS.GetLegSuppoftFlagO; 

//  prints  body  and  leg  xyz  ooordioates 

/• 

int  tow,  ool; 

printfl[*lcgl  leg2\o”X 

fof(  row  =  1;  row<3;  row++){ 

for(col  •  3;  ool>0;  col-) 

printfl;’^.4f  "4C->leglcP  *  row  •  colj); 

■); 

fof^col  =  3;  col>0;  cd-) 
piiiitf(*%6.4f  "4»^leg2c[3  •  row  -  col]); 
printfIT'n"); 

} 

prwtflT'n"); 

printflTi^  Ieg4>n"): 

foifrow  ”  1;  row<5;  row++){ 

forfcol  “  3;col>0;  col-) 

printH’'346.4f  "^>leg3c[3  •  row  -  col]); 
printfC  "); 

foif  col  “  3;  ool>0;  col-) 

priiitfl;"%6.4f  "^>leg4cp  •  row  -  col]); 
printff^”); 

} 

printfl7\n'>, 

printfl["le^  leg|6'n'); 

forfrow  =  1;  row<5;  row++){ 

fof^col  “  3;  col>0;  col-) 

printlC^ftS.df  "^>leg5cp  *  row  -  col]); 

printr  "); 

for(col  =  3;  ool>0;  col-) 

printfC^d.df  "4ic->leg6cP  •  row  -  cot]); 
printf("\n"); 

} 

printf|["\n7, 

•/ 

return  •rc; 

} 
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// FUNCTION:  rile_U$e 
// PURPOSE:  rcttb  dewMi  leg  dMnges  from  a  file 
// INPUT:  reedi  fiom  file:  Ie|^,  deitel,  deiU2,  <lelU3 
//  OUTPUT:  celctiletw  new  le^Unk  ooardinatet  (mienial) 

Paating;_Iteim  File_Use<FILE  *i4>>  A<|iiarobotBoily  Abody, 

AquaLeg  ftlegl,  AiiuaLeg  &le^  Aqual^  &leg3. 
AquaLeg  Alcg4.  AquaLeg  AlegS,  AquaLeg  &leg6) 

{ 

Passing_Itenis  *pass; 
pass  new  Passiag_Items; 


ficanf(i%'9id  %lf  %lf  t«lf  Slf  Sif  Sif  .ftpass->body,&pass->ddl. 

ftpasS'>del2.ftpasa-^l3,&pa»->deI4,&pa»->delS,&pass->del6); 


if  (pass>>legnuni  <  9)  { 

body.MovieInarmfnlal(pats->dell.  pass->det2,  pass->det3, 

pass->del4.  pass->delS,  pass->del6); 


ficaIlf(i4^*%d  SIf  Slf  9Uf . 

&pass->legniim,  &pass->dell,  ftpass->del2,  &paas->del3); 
legl.Movelncrenieoal(bodyjiass^>dell4>ass-^l2,pass->del3); 
fican^iip,'^  %tf  SIf  Sir. 

&pass->leg|iuin,  ftpass->dell,  &pass->del2,  &pass->del3); 
leg2.MoveIncran«Ul(body,pass^>dell4iaes-^l24>ass-:^l3); 
ficaiifi;i4>,**/4d  ^Slf  9Ur, 

&paas->legnuin,  &pass->dell,  &pass*><lel2,  &pass->deD); 
leg3.MoveInGranental(bodyj>ass->dell4>ass-:^l2,p«95->deD); 
ficaiifi:ifp,'^  %if  WSIf , 

&pass->le9ium,  &paas->dell,  &pass->del2,  &pass->deU); 
leg4.MoveIncranea<al(body4)ass->del  1  jMss-::^124>iss->deDX 
fiKaiif(i4>.'%d  «4lf  WSir, 

dipass->legniiin,  &pass->dell,  ftpass->tid2,  &pass->del3); 
leg5.MoveIncRnieotal(body,pass->del  1  j)ass-^^l2,pass->del3); 
BauU(Up,-%d  HIT, 

d^>ass->legniini,  ftpass->dell,  &pass-><ieI2,  Apass^dcD); 
le^.MoveIncraneidal(bodyjMss->del  1  jMss-^l24>ass->del3); 
ficanfTiQi.'fid  •Alf  %lf  SIT, 

ftpas»->legnum.  ftpass->dell,  &pass->deI2,  &pass->deD); 


if  (pass->legiiiim  =  0)  { 
pass->dell  =  0.0; 
pass->del2  -  0.0; 
pass->del3  =  0.0; 

}; 

}; 

retuni  *pass; 

} 


//FILENAME:  Ttansfia-ToGait 

// PURPOSE:  places  the  body  center  and  leg  coordinates  in  a 
//  Next  Motion  stnicture  fix  gait  algorithm  use 

Next  Motion  TransfoToGait(Retura  Coordinates  &ooord,  AquarobotBody  Abody) 

{ 

Next_Motion  •terap; 
tenq>  =  new  Next_Motion; 


tenq)->body_center_coord(3]  =  ooord.bodyc[0]; 
temp->body_oenter_ooord[4]  =  ooord.bodyc[l]; 
tenq>->body_oenter_coord(5j  =  coord.bodyc[2]; 


tenq>->lbot_l_coord[0]  >  coord.leglc[9]; 
temp->foot_l_coord[l]  =  coord.Ieglc[10]; 
temp->fi>ot_l_coord[2] «  coafd.leglc[ll]; 
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tciiip->feol_3_cooni(0]  •  coonllej;2^9]; 
tanp-^>foat_2_caoni{l]  -  coard.lcg2c{10]; 
tcai|>->fi>at_2_ooard[2]  ••  ooard.le^c(ll]; 

teDip->feot_3_oocrd{0] »  ooanlleg3c{9]; 
tcmp->fi>at_3_ooard(l]  -  ooatdlcg3c(10]; 
tcin|>->foot_3_ooafd[2]  >  ooard.le83c(U]; 

taiip->fi)ol_4_ooad{0]  >  ooatdleg4c{9]; 
tanp-^>foot_4_coanl(lj  •  oaonLleg4c(10]; 
tcnp->fi>at_4_coard[2j  -  oaard.leg4c[ll]; 

teiiip->foot_S_coaiid[0]  •  ooord.IegSc[9]; 
tetnp->fi>ot_5_coo(d(l  j  •  coordlegSc(10]; 
tcmp->foat_3_coanl(2]  =  coord.  teg3c(U]; 

teinp->feoC_d_coofd{0]  -  cootd.ieg6c(9]', 
tcnij>'>feot_6_coani(l]  >  coard.lcg6c{10]; 
tenif>->foot_6_coord{2]  -  coonLlcgdcIl  1]; 

//  get  joint  angle  values  from  algoritfam  module 
for  (int  i-0;  i<d;  i++)  { 

for  (ini  j"0;  j<4;  j++)  //  HIP  ->  FOOT 

temp^>inbdJoint_angle[i][j]  =  coord-inbdjoiid  attgle[i][i]; 

}; 


//  current  body  elevation 

tenip->body_oenter_coord(l] « -1.  •  body.H_matiw->val(2,0); 
//  current  body  azimuth 

leinp‘>body_oeater_ooord[0]  >asin(body.H_matrix->val(l,0)/ 


//  current  body  roll 

tenq>->body_center_coord(2]=asin(body.H_matrix->val(2,l)  / 


//  joitd_limit_fUg 

teinp-^oint_titnit_flag(0]  •  ooaidjnotioa_liniit_flag(0]; 
temp-^int_limit_fiag[li  <•  cootdjnolica_limit_ilag(li; 
tenip-^oint_liniit_flag[2]  *  ooofd.molion_limit_flag(2]; 
tenip-^oiid_limit_flag[3]  oooid.mattoa_limit_flag(3  j; 
temp->joint_limit_fIag(4j  -  ooard.matiaa_limit_fiag(4j-, 
tan(>->ioint_litnit_flag(Sj  ~  ooord.iiiotioa_liniit_flag(3j; 
temp-^oint_liniit_fUg[6]  -  ooord.malion_limit_flag[6]; 
temp->joint_limit_flag(7]  -  ooard.motian_limit_ilag(7]; 
temp->joint_limit_flag(8]  -  coord.mocion_limit_flag[8]; 
temp->joint_limit_flag(9]  “  oootxlraotion_liniit_fUg(9j; 
teinp->ioint_limit_fUg(10]  •  coardjnotiaa_limit_fl^lO} 
tein|>-''joint_liinit_fUg(l  1]  •  coord.motioii_liniit_flag[l  1] 
temp->joint_limit_flag[12j  -  Goardjnation_liniit_flag(12] 
teinp->joint_limit_fIag(13j  •  coord.niotioo_limit_flag(13] 
teTnp->jointJimit_flag[14] «  coord.niotioo_lifflit_flag[14} 
teinp-^oint_liniit_flag{13]  coanLma(ion_limit_flag(IS] 
temp->joint_limit_flag[16]  •  ooard.motiao_limit_fUg{16] 

//  leg_contact_flag 

lemp->leg_cootact_ilag{0]  -  coord.leg.suppott_flag(0]; 
temp->leg_contact_flag(l]  -  ooord.leg^suppait_flag(lj; 
tem{>->leg_coiilact_flag[2]  =  coord.leg;_iupport_flag(2j; 
tetnp->leg_contact_iIag(3]  -  coord.leg^support_flag(3]; 
teinp->leg^contact_llagJ4]  -  coord.leg,siq)poit_ilag(4]; 
ten^>leg_contact_fUg(S]  =  coord.leg_suppott_flag[}j; 
tenip->leg_contact_flag[6]  “  ooord.leg^support_flag(6j; 

return  *temp; 


cos(temp->body_center_coord[  1  ])); 


cos(t*nip->body_ceiiter_eooid(  1  ])); 


225 


} 

#ifiidefKINEMATICS  H 
#defiiie  KINEMATICS.H 

//  FILENAME:  KwiwnitirrH 

#mdude  "arcaatH'' 

^include  "MabixMy.H" 
#indude*AbLeg.H' 
itmOaic  'AbBmfy.H* 

//  stnicture  deaigped  to  receive  file  in|)ut 
struct  Paning_Itemi  { 
iialegDuni; 
ittbody. 
double  dell; 
double  del2; 
double  del3; 
double  del4; 
double  del5; 
double  del6; 

}; 


// structure  desigoed  to  return  the  xyz  coordmates  the  robot 
struct  Returo_Co(xdinates  ( 
double  bod^[21]; 
double  leglc(12]; 
double  le^|l2]; 
double  leg3c[12]; 
double  Ieg4c[12i; 
double  Ieg3c(12]; 
double  iegdc{12]; 
im  tnotk»_liniit_flag(18]; 
iot  leg_su|i|>art_flag[6]; 
double  aiigle[6][S]; 

}; 


//  structure  to  recieve  next  robot  motion  fiom  gait  pfaumingalgorttfams 

struct  Next_Mati<n  { 

public: 

double  bodyniotioa[6]; 
double  leglmotioop]; 
double  leg2inoti<]a[3]; 
double  legSmotionpj; 
double  kgdmotionp]; 
double  leg5tnotioa(3j; 
double  l^motionpj; 
int  leg,coiilact_flad6]; 
int  joint_liinit_fl^l8]; 
double  inbdJoiiit_ang|e[LEG][S]; 
double  foot_l_coord[3]; 
double  fi)ot_2_cootd[3]; 
double  fix)t_3_ooatd{3]; 
douUe  fi>ot_4_ooord[3]; 
double  foot__S_ooord[3]; 
double  fi>at_6_coord[3]; 
double  body_oeiiter_oootd[6]; 

//constructor 
Next_MotiooO  { 


bodyniotioo[0]  >  0.0;  bodyinotion[l]  ~  0.0;  bodymotion[2]  =  0.0; 
badyn)aCion[3]  =  0.0;  bo<fyinotion[4]  0.0;  bo<^inotioo[}]  ^  0.0; 
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kg_caaUct_fla((L£Gl]  >  1;  l«g_oaalact_flag[L£G3]  ~  1; 
leg;_ooaUGt_Sag[LEG3]  >  1;  kg;_oaB(act_flag(LEG4]  >  1; 
lejL.caaiact_flag(LEGS]  •  1;  lf^oaatact_flag(L£G6] «  1; 


teglmolioiiPQ  °  0.0;  tegliiiotiaii{Y]  >  0.0;  leglinotioo(Z]  0.0; 
k^ZmotkiiiPC]  •  0.0;  le^2molicMi(Y]  °  0.0;  itglncttki^Z]  “  0.0; 
leg3motioa(X]  >  0.0;  teg3iiiotiflo(Y]  =  0.0;  leg3iiiotioii[Z}  -  0.0; 
IqptaKMiaapq  >  0.0;  leg4maliaii[Y]  >  0.0;  1^4iDO(ioti[Z  j  0.0; 
tegSmotioiipC] »  0.0;  leg3mation|Y]  ~  0.0;  legSiiiotioa{Zj  =  0.0; 
leg6mo(ion[X]  >  0.0;  leg|6fflo(iaii(Y]  ~  0.0;  le86iiioti<»(Z]  -  0.0; 


joint_Iiiiiit_fUg(0] »  0;  joint_liiiiit_flag(l]  -  0; 
joiiit_ljinit_flag[2]  -  0;  joint_tiniit_flag{3]  -  0; 
joiat_liiiiit_flag[4]  -  0;  joia_liiiiit_flag[5]  -  0; 
joint_liinit_flag(6]  -  0;  joint_liinit_fUgI7]  -  0; 
joint_liinit_flag(81  -  0;  joim_lmiitjtUg{9]  “  0; 
joiiit_limit_fUg[10]  -  0;  joiot_liiBit_fUg{ll] «  0 
joiat_liniit_fl<g[12] «  0;  joiitf_tiiait_flag(13]  “  0 
joiat_liiiiit_fU8{14]  >  0;  joii]t_Ufflit_fUg(13j  >  0 
joint_]iinit_flag[16j  >■  0;  joiiit_liinit_il«g(17] »  0 


ii]ixlJoint_angle[L£Gl][CB]  =  (double)LEGl«60.0; 
iiibdJoint_aiigle[LEGl][HIP]  ’°St«itTh^l; 
iiri)dJoii4_aiigle[LEGl][KNEEl]=  StaitTheu2; 
■nbdJoiiit_tii8le[LEGl][KNE£2j>  SUrtTheuG; 
iiibdJoiiit_*iigle[LEGl][FOOT]  =  0.0; 

iEMJ<>iiit_«igle[LEG2][CB]  >  ((k>ub><!)LEG2*60.0; 
ndidjoint  aiig]e(L£G2][HIPl  ^StatTlieUl; 
ifibdJoat3*<>glc(LEG2j[KNEEl]-  StattTheU2; 
inbdJoint_*iigle[LEG2](KNEE2]’=  StaitTbeU3; 
iiibdJoiiit_«n8le{LEG2](FOOT] «  0.0; 

inbdJoiia:_Migl*{LEG3][CB]  -  (double)LEG3*60.0; 
iobdjoiat  «cig|e{LEG3][KlP]  ^  SUrtHieUl; 
mbdJoifltIaiigle[LEG3](KNEEl]>  St«tTbeu2; 
iiibdJoiot_*ngle[LEG3][KNEE2j-  StittTbeta3; 
iiibdJoint_,angle{LEG3][FOOTl  “  0.0; 

inbdJoint_aiigle(LEG4](CB]  >  (double)LEG4*60.0; 
inb(lJoint_angle(L£G4][HI^  -SUrtTb^l; 
iiibdJoiiit_angle[LEG4][KNEEl]~  SUrtTheU2; 
iiibdJoiiit_angle[LEG4][KNEE2]>‘  Stiit'nieU3; 
inbdJoiiit_aii8le[LEG4][FOOT]  =  0.0; 

iidKlJ<»iit_angl«[LEGS](CB]  >  (double)LEGS*60.0; 
iiibdJoiiit_angle[LEG3][HIP]  ^StaitTheUl; 
inbdJoiiit_«ngle{L£G3][ICNEEl]=  SttttTheU2; 
ii)bdJotiit_«ngle(LEGS][KNEE2j=  SUrtThda3; 
inbdJoiiit_«ngle[LEG3j[FOOT]  =  0.0; 

iiW>d joiiit_«ngIe[LEG£][CB]  =  (douUe)LEG6*60.0; 
iiibdJoiiit_aii^e[LEG(][HIF]  ^StaitTh^l; 
iiibdJomt_aii^e[LEG6][KNEEl]>  SUitTheU2; 
iiibdJoint_«ngle[LEG6][KNEE2]>  StartTheta3; 
inbd Joiiit_*ngle(LEG<][FOOTJ  -  0.0; 

/*  respect  to  WORLD  Coonttnates  */ 

foot_l_coonipCW]=  98.02;  fbot_l  coorf[YW]=  0.0;  foot_l_<»ordIZW]=0.0; 
foot_2_coonJ{XW]=  49.01;  foot_2  eoonllYW]- 84.88781;  foot_2  eoonl[ZW)=0.0; 
foot_3_coordpCW]=-49.01;  foot_3~coordfYW]»  84.88781;  foot_3  cooni[ZW]=0.0; 
foot_4  coordpfW]=-98.02;foot  4  coord(YW]»  0.0;  foot_4_co«dIZW]=0.0; 
foot  5“(»o«ipCW]-^9.01;foot“sIcooritYW]=-84.88781;foot_5  coord[ZW]=0.0; 
foot* 6_coordpCW]=  49.01,  foot"6_coord[YW]=-84.88781;  foot_6  coord[ZWl=0.0; 


body_ceiiter_eoord[0]  =  0.0; 


bo<i^_oa<er_ooanl[l]  ~  0.0; 
body_ocnlcr_ooanl{2]  >  0.0; 
body_otnlcr_ooard{3]  ~  0.0; 
body_ccnter_oaatd[4]  -  0.0; 
body  ocolcr  ooanl[S]  >  -TO.Tl; 

}; 

}; 

//stnictiite 

sinict  WaUd’anmeter  { 
public: 
ini  phase; 
double  atrkle; 
double  directioa; 
douUetiipodiizr, 
double  footbeigfal; 
double  bodyapeed; 

WaDcFarameterO  { 
phase  >0; 

stride  •  OeiaullSTRIDE; 

direction  >  0.0; 

tripodsize  =  98.02; 

footbeigfat  -  DefaullFOOTheight; 

bodyspeed  >=  (De£uiltSTRIDE/2.oyFINE; 

>; 

}; 

//alniclure 
struct  Flag  { 
public: 
mtphaaeEad; 
intgoal; 

// coostiuclor 

FUe0{ 

phaseEnd-O; 
goal  =  0; 

}; 

>; 

extern 

Retum_Coofdinates  FindPasitiai]s(A<piarobolBody  &, 

Ai]uaLeg  &,  AtpiaLeg  Aq[uaLeg  &, 
AquaLeg  &,  AquaLeg  A,  AquaLeg  A); 


extern 

Passing_Iteins  File_Use(FILE  *,  AquarobotBody  A, 

AquaLeg  A,  AquaLeg  AquaLeg  A, 
AquaLeg  AquaLeg  AquaLeg  A); 


extern 

Next_Motion  TransferToGait(Retum_Coordinates  &,  AquarobotBody  A); 
#eixlif 

// FILENAME:  LiidcO.C 

//  PURPOSE:  Declaratioos  fix'  class  LinkO 

#iiiclude  "LinkO.H” 

Linlc0::LinkD0 :  Liric  (  0, 37.5, 0.0, 0.0, 0.0,  -1.0, 

-360.0,360.0) 

{ 

node_list  •  new  n]atrix(4,4,0.0); 
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node  liit->va](03)  ~1-;  i>ode_liit>>v*J(l^) -1.; 
iiodeIli*t->viK2.0)  -37.3;  node_liit->veK23)  .  1.; 

T_niatnx  new  inatnx(4,4,0.0X 

} 

// FILENAME:  LiidiO.H 
//PURPOSE:  DecUreOom  for  dag  tinfcO 

MiSadetH  USKO 
iMefineH_lJNKO 

#include  'Link.H' 

cUas  UnkO  :  public  Link 

{ 

private: 

public: 

LiidtOO; 

}; 

#cndif 

//FILENAME:  Linkl.C 


kinchide*Liidcl.H’ 


//  Linkl:;LinklO :  Link  (  0. 20.0.  -90.0, 60.4, 0.0, 0,-I06.6,73.4) 
//  Changed  by  Keoji  Suzuki  A  Chuck  Sduie 
Linki::Linkl0 :  Link  (  0. 20.0,  -90.0, 33.86, 0.0, 0,-106.6,73.4) 
{ 

node_Uit  -  new  inatrix(4,4,0.0); 
node_Iiat->val(03)  -L; 
node  list->val(I,3)  •!.; 
node~list->vaK2,0)  -  20.0; 
node_list->val(23)  -  L; 

T_matrix  =  new  niatrix(4,4,0.0); 


> 

//FILENAME:  LinkLH 

//  PURPOSE:  Dedaiatioos  for  class  LinkO 

#ifodefH_LINKl 

#de&ieH_UNKl 


^include  "LialcH' 


class  Linkl :  public  Link 

{ 

private: 

public: 

LinklO; 

>; 


#endif 
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//  FILENAME:  Lii*2.C 
//PURPOSE:  DecUralioacfiirciaMLiidiO 


#inch«le'Liiik2.H* 

//  linlr^-IJnlr^O ;  Lii*  (  0. 50.0. 0.0.  -15S.4. 0.0. 1.0.  -156.4. 23.6) 
//  riiMged  by  Kcnji  Suzuki  &  Chuck  Schue 
linH-liiAlO :  LinkC  0. 50.0. 0.0.  -125.86. 0.0. 1.0.  -156.4. 23.6) 
{ 

nodeliit  new  matrix(4,4.0.0). 
iK)de_list->val(03)  “!•;  node_li*>vil(13)  “1.; 
iiode_U*t->v«K2.0)  -50.;  oode_li«->vil(23)  -  1-; 

T_iiiatrix  -  new  initrix(4.4.0.0). 


} 

//  FILENAME:  Liiik2.H 

//  PURPOSE:  DecUntiau  for  cU«  LinkO 

#ifoikfH_LINK2 
MeSae  H_UNK3 

#iiiclu<le*Liidcir 

clan  Liiik2  :  public  Liidc 

{ 

private: 

public: 

Link20: 

}; 

#«ndif 

//FILENAME:  LinkS.C 

//  PURPOSE:  DeclaratioM  for  clan  LinkO 


#iiiclude  'Link3.H’ 

Link3::Link30 :  Link  (  0. 100.0. 0.O.O.0, 0.0. 2.0.  -360.0^60.0) 

{ 

node_list  -  new  matrix(4.4.0.0); 
node_list->val(04)  =L;  node_li*t->val(1.3)  =1.; 
node_list->val(2.0)  -100.;  node_list->val(2.3)  -  1.; 

T_matrix  -  new  matrix(4.4,0.0); 


} 

// FILENAME:  Link3.H 

//  PURPOSE:  Declarationf  for  clan  LinkO 

//*.....**•..**«•*....*«***».*••****■ 

#ifedefH_LINK3 

#defineH_LINK3 

ilinclude''Link.H’ 
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cbMLink3  ;  pid>lic  Link 

{ 

phvate: 

public: 

Liak30; 

>; 


#aidif 

//  *•••••**«••••••••••««••••*••••«••«•••••««*•••«•*•••«* 

//FILENAME:  LitJcC 

./  PURPOSE:  fanplcnienlatMn  of  d«a  Ln^ 

//  CONTAINS:  UpdateAMatrix  0 
//  Rotate  (double  angle) 

//  RotateLink  (double  angle) 

jj  «««*««•««•«*••«*««««««««««•««•«•«««••««••««•«•««•««•• 


#mcliide'Link.H” 


const  int  Tnie  1; 
const  int  False  ~  0; 

//FUNCTION:  Link 
//PURPOSE:  Condnictor  for  Link 

Link::Link  ( int  mIL  double  U.  double  ta,  double  ija,  double  ijd,  double  iL 
double  min_ia,  double  max  Ja ) 

{ 

motion_limit_fUg  >  mlE 
liok_lenglh  •  11; 
twist_aog|e  •  ta; 
inboardJoint_angle  >  ija; 
inboard J<Mat_displacenimt « ij^ 
inboard_link  =  il; 
minJoint_angle  minja; 
maxJoint_angle  =  maxja; 

H_matrix  =>  iiewniatiix(4,4,0.0X 


H_matrix->UpdateTMatiix(ija,ta,U,ijd); 

) 

//FUNCTION: -Link 
//  PURPOSE:  destnicior  for  Link  class 

Link::~LinkO 

{ 

delete  T_matrix; 
delete  node_list; 

} 

//FUNCTION:  Rotate 

// PURPOSE:  rotates  a  Liidc  by  changing  the  T  Matrix 
//  by  the  inboard  j<»n  angle  desired 

//******«•*••*•*.«**•.*««•******•«.*.*.*...*«.«•***•«.*.. 

void  Link::Rotate  (matrix  *mat,  double  angle) 

{ 

SetInboardJoiiitAngIe(angle); 

T_malnx->UpdateTMatiix(GetInboardJointAn^eO,GetTwistAn^eO, 

GetLinkLenglh0,GctInboardJointDi9lacement0X 

// the  "mat”  is  the  inboard  link's  A  matrix  (or  the  body's 
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// A  oHltix  for  the  aboard  joint 
*H_aaliix  -  •mat  •  •T_mttix; 


} 


//  FUNCTION:  RotatcLak 

// PURPOSE:  dctcrmtiws  if  the  ratatioo  ia  withia  physical 
//  joint  oaaatninls.Ifoulaide  the  woifcapaoe  the  min 
//  armaxUniitappliGaUciauaed. 

//  thiafuoclioDcaUa  the  Rotate  fonctioa 


void  Link::Roiaelink(niatrix  *nud.  double  angle) 

{ 

double  tcaler, 

teatcr  -  CetMinloiatAogleO; 

if  (angle  <  teatcr)  { 

angle  •  teeter, 

SetMotionl  jinitFlag(  1); 

} 

teeter  -  GetMaxIointAngleO; 

if  (angle  >  teeter)  { 

angle  •  teeter, 

SetMolicoLiinitFlag(lX 

} 

Rotale<mat,  angleX 

} 


//FILENAME:  Link.H 

//  PURPOSE;  Oeclaratiow  for  clan  Liidc 

//  COMMENTS:  Definitian  of  Link  claas 


MfodefH  LINK 
MefineH.UNK 

#iiKtude  <aUtio.h> 
Hinchide  <inatli.li> 
#iiiclude  *AbRigid.H* 
//include  'MatrixMy.H” 


clasa  Link:  public  RigidBody 

{ 

private: 

iid  niotion_tiiiiit_flag; 

double  link_leoglh; 

double  twiet_angle; 

double  idboatd Joint_angle', 

double  inboard  Joint_displaoenient; 

double  inboard_link; 

double  min  Joint_angie;  //  rotary  link 

double  max  joiin_angle;  //  rotary  link 

public 

void  RolateLiak(matrix*,  double); 


matrix  •T_nialrix,  *node_litt,  •H_mattix; 


Liiric  ( iid  mlf  double  IL  double  ta,  double  ija,  double  ijd,  double  U, 
double  min  Ja,  double  max  Ja  X 


-Link(X 


bit  GctMotionLnnitFlagO  {return  motion_lbnit_flag;} 
double  GetUnkLenglhO  {return  Imkjenglh;} 
double  CelTwhtAngleO  {return  twist_angle;} 
double  GetlnboardJouitAngleO  {return  biboard Joiiit_angle;} 
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double  GetlnboerilJointDiyUceniertO  {tcbini  ii)bowdJoiiit_dispUc«ment;} 
double  GcUnboardlinlcO  {retaun  iiiboard_link;} 
double  GetTMatrix(iiit  row,  iol  col)  (rctuni  T_inalrix->val(row,coI);} 
douUe  GctMmlointAiigteO  {return  min Joiin_angle;} 
double  GctMaxJointAngleO  {return  nuxJoint_angte-,} 


void  SetMotiooLimitFUg(iin  «)  {niotian_limit_fUg  =  a;} 

void  s»«t  jnlct  <»igrti(A»ihle  a)  {liiik_lenglh  ==  a;} 

void  SetTwiitAogic(dauble  a)  {twiat_angle  >  a;} 

void  SelInboanUondAngle(double  a)  {inboard Joiot_aiig|c  =  a;} 

void  SetInboatdJaintDiaplaceaicni(double  a)  {inboafd_joint_diq>Uoenieat  =a;} 

void  SetMioardLiidcCdouble  a)  {iiiboard_linlc  a;} 

void  SetTMaiiui(int  row,iDt  double  a) 

{T_i,*alrix->vaJ(row,col)  “  a;} 
void  SetMinJointAngJe(double  a)  {niinJoint_an£|e  =  a;} 
void  Setb  ^ax)oinLAngle(double  a)  {maxJomt_angle  >  a;} 

void  RoUte(mattix*.  double); 

}; 

#endif 

//  FILENAME:  MatiixMy.C 

//  PURPOSE:  Implementation  of  MatrixMy  clan 

//  CONTAINS: 

#include  <ndio.b> 

^include  <stdlib.h> 
itinclude  <aliing.b> 

#ioclude  <math.h> 
ttinclude  "MalrixMy.H’ 

#include’AbLefrH'' 

matrix:  analiixO 

{ 

p  •  new  matrep; 

p->r-4; 

p->c-4; 

p->m  ”  new  double  *[4]; 
intx; 

for  (x  *0;  x<4;  x++) 

p-^>m[x]  •  new  double[41]; 

p.>n=-  1; 
intj; 

for  (int  i”0;  i<4;  i++) 

for(j”0;j<4;j++) 
p->m[i][j]  =  0.0; 


matrix:3natiix(mtrows  -  1,  int  col  =  1,  double  initval  =  0.) 

{ 

p  =  new  matrep; 

p->r*rows; 

p->c  ”  col; 

p->m  ~  new  double  *[rows]; 

iinx; 

for  (x  =0;  x<rows;  x-H-) 

p->m[x]  =  new  double[col]; 

p->n=  1; 

intj; 

for  (int  i=0;  i<rows;  i++) 

for  j-'-'-) 

p->m[i][)]  =  initval; 
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inathx:3iiatrix(coiift  i>ialnx&  x) 

{ 

x.p->irv+; 

P  -  x-p; 

} 

matrix  matrix: '.opentorKoonst  matrixft  rval) 

{ 

if(-p->n“0)  { 

^  (int  x><l;  x<row»0'. 

delete  p->m[x]; 

delete  p->m; 
delete  p; 

} 

rvaLp->ii-t-+; 
p  «  rval.p; 
return 

} 


matrix;:-matiixO 

{ 

if(_p.>n  =  0)  { 

for  (int  x»0;  x<ro\vsO;  x++) 
delete  p->m[x]; 
delete  p*>m; 
delete  p; 

} 

} 

double  &  matrix;  :val(iiit  row,  iot  col)  const 

{ 

return  (p->m(tow][col]); 

} 

matrix  matrix:;operator*(coiist  inatrix&  arg) 

{ 

matrix  resutt(rowsO,arg.colsO.O.O); 
for  (bit  row=0;  iow<rowsO’.row++)  { 
intooU 

for  (col=0;  col<arg.colsO;  co1-h-)  { 
double  sum=0.0; 
for  (bit  i=0;  i<colsO;  i++) 
sum  +=  p->m[row][i]  •  arg.val(i,col); 
result  val(row,col)  =  sum; 

} 

} 

return  result; 

} 

matrix  matrix;  :operator*(double  a) 

{ 

matrix  result(rowsO,colsO,0.0); 
for  (bb  M;  i<rowsO;  i+-^)  { 

for  (int  j»0;  j<cols0;  j++)  { 
double  ans; 

ans  =  result  val(ij)  •  a; 
result  va](ij)  =  ans; 

} 

} 

return  result; 

} 

matrix  matrix:  :operatorKconst  matrix&  arg) 

{ 

matrix  sum(rowsO,colsO,0.0); 
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for  (int  i-0;  i<rowiO;  i++)  { 
iatj; 

for  (j”0;  j<col*0;  j-t-+) 

sum.p->ni[i][j]  “  p->in[i][j]  +  ifK.val(iJ); 

} 

return  sum; 

} 

void  matrixi.'priidO 

{ 

for  (int  row«0;  row<row(sO;  row++)  { 
int  col; 

for  (col=0;  col<cols(X  tol+  i-) 

prinlf(*%6.6f ",  ivO>in(row][coll); 
printf("\n">. 


matrix  &  matrix::HomogeneousTnns£iirm(double  azimuth,  double  elevation, 
double  roll, 

double  X,  double  y,  double  z) 

{ 

double  spsi  =  sin(azimuth); 

double  qni  cos(azinuith); 

double  sth  >  sin(elevatioo); 

double  cth  >  cos(elevation); 

double  sphi  ~  sia(roU); 

double  cphi  =  cos(toU); 

val(0,0)  ”  (qni  •  cthX 

val(0,l)  =  ((qiti  *  sth  *  sphi)  •  (spsi  *  q>hi)); 

val(0,2)  =  ((qisi  •  sth  •  cphi)  +  (spsi  •  sphi)); 

val(03)“x; 

val(l,0)  =  (spsi  •  cth); 

val(l,l)  “  ((cpsi  *  cphi)  +  (spsi  •  sth  •  sphi)); 

val(l,2)  *  ((qisi  *  sth  •  cphi)  -  (cpsi  *  sphi)); 

val(U)“y, 

val(2,0)»(-sth); 

val(2,l)  =  (cth  *  sphi); 

val(24)  =  (cth 'cphi); 

val(2,3)-z; 

val(3,0)  =  0.0; 

val(3,l)  =  0.0; 

val(3,2)-0.0; 

val(3,3)  =  1.0; 

return  *this; 


matrix  &  matrix:  :DHMatrix(double  cosrotate,  double  sinrotate, 
double  costwist,  double  sintwist, 
double  length,  double  translate) 

{ 

val(0,0)  =  cosrotate; 

val(0,l)  =  -1  *  sinrotate; 

val(0,2)  =  0.0; 

val(0,3)  =  length; 

va](l,0)  =  sinrotate  •  costwist; 

val(l,l)  =  costwist  *  cosrotate; 

val(l,2)  =  -1  •  sintwist; 

va](l,3)  =  translate  *  -1  *  sintwist; 

va](2,0)  =  sintwist  *  sinrotate; 

va](2,l)  sintwist  *  cosrotate; 

va](2,2)  “  costwist; 

val(2,3)  =  translate  •  costwist; 

val(3,3)=  1.0; 
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rdurn  *liiis; 

> 

Matrix  ft  ■iiatrix::UpdateTMririx(douUe  a,  douUc  b,doubU  c,  double  d) 

{ 

a  »  a  •  deg_to_nd; 
b  “  b  •  deg_to_rMl; 
double  oovotate  <■  ooe(aX 
double  sinrotale  -  stii(a); 
double  ooatwist  >  coc^); 
douUe  liiitwist  siii(bX 

DHMatrix(co«oUte,  siiirolate,  coetwixt,  tintwiit,c,d); 


fetuni*lliis; 

} 

matrix  ft  iiiatrix::TraiiafoniiNodeLiat(aiatrixftH  matrix,  matrix  ftb) 

{ 

matrix  tem|i(4, 1,0.0); 

foe  (int  i  “  0;  i<b.rovnO'> »++)  { 

tefiip.val(0,0)  =  b.val(i,0); 
temp.val(l,0)  *  b.val(i,l); 
temp.val(2,0)  •  b.val(i,2); 
temp.val^,0)  ”  b.val(i3); 
matrix  middle  °  H_matrix  *  leap; 


val(i,0)  -  middle. val(0,0); 
va](i,l)  -  middle. val(l,0); 
val(i,2)  -  middle. val(2,0); 
val(i3)  =  middle.  val(3,0); 
}; 


retuni*this; 

} 

matrix  &taMtnx::Truisf<xmBodyLisl(inMtrix&H  matrit;  matrix  ftb) 

{ 

matrix  tcmp(4,l,0.0); 

for  (iid  i=0;  i*i.roMwO;  { 

temp'.\«l(0,0)  =  b.^(i,0); 
teii9.val(l,0)  =  b.val(i,l); 
tenap.val(2,0)  «  b.val(i,2X 
temp.val^,0)  =  b.val(i,3); 
matrix  middle  -  H_mxtrix  *  temp; 


va](i,0)  =  middle.  val(0,0); 
val(i,l)  =  middle.  val(l,0); 
val(i4)  =  middle.  val(2,0); 
val(i,3)  =  middle.  valp,0); 

}; 

return  *tliis; 

} 

//  FILENAME:  MatrixMy.H 

//PURPOSE;  To  provide  for  a  matrix  class  to  acGonqiluli 
//  some  necessary  robotic  and  kinematic  needs. 

//  COMMENTS:  DHMatrix,  Homogeneous  Transform,  and 
//  TransformNodeUst  ate  included 

//a*..*.*********..*****.**..*****..*...***..**.*.... 

#ifiidefH_MATRIX 

#de&ieH_MATRlX 


const  double  deg_to_rad  =  .017453292519943295; 
class  matrix  { 
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Unict  matrap  { 

>V. 

public: 

iiiitnx(ooiist  matrixft  x);  //copy  initializer 

-matrixO; 

maliixO; 

malrix(int,  int,  double  ); 

mabix  operator>=(coost  matiixft  rval); 

matrix  operatorKconat  matrixdl  rval); 

matrix  operator*(coait  matrixft  rval); 

matrix  operator*(double); 

double  ft  val(iiit  row,  int  ool)ooiiit; 

voidprintO; 

int  rmwtO  oonit  {return  p->r,}; 
int  oolaO  (return  p->c;}; 

matrix  ft  HamogeneouxTrat«ifiicni(double,double,double,double,doubie,double); 

matrix  ft  DIlMatrix(double,  douU^  double,doubie,  double,  double); 

matrix  ft  l^)dateTMatrix(d^te,  d^te,  double,  double); 

matrix  ft  TtaiisfomiNodeLiat(malrixft,  matrixft); 

matrix  ft  TianafotmBodyf  iri(inatrixft,  ituuixft); 

}; 


#endif 
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APPENDIX  D: 


AQUAROBOT  SIMULATOR  USER’S  MANUAL 


This  appendix  contains  the  information  necessary  to  use  the  version  of  the 
AquaRobot  Simulator  working  at  the  time  this  thesis  was  written.  The  User’s  Guide  was 
intended  as  a  stand  alone  document;  it  may  be  removed  from  the  thesis  and  used 
separately. 
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AQUAROBOT  SIMULATOR 
USER’S  GUIDE 
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PREFACE 


PURPOSE  OF  THE  MANUAL 

This  manual  provides  user-specific  information  necessary  to  operate  the 
AquaRobot  Simulator  on  the  Silicon  Graphics  Personal  Iris.  Code  modification  details  are 
purposely  excluded. 

KNOWLEDGE  LEVEL 

Some  knowledge  of  the  Silicon  Graphics  workstation,  the  UNIX  operating  system, 
and  the  mouse  pointing  device  is  presumed. 

CONTENTS 

l.  Logging  On 
n.  Compiling 

m.  Using  the  Simulator 

Attachment  A;  Flowchart  and  Function  Descriptions 
Attahcment  B;  Required  Files  Listing 


241 


L  LOGGING  ON 


Step  1 ;  At  the  Login  screen,  type  in  the  appropriate  responses  to  the  Login  prompts. 

Login,  aquarobo 
Password  .  _ 

The  password  is  available  from  Dr.  Kanayanna  or  Dr.  McGhee.  Once  Login 
is  completed,  the  UNIX  operating  system  is  loaded. 

Step  2:  Click  on  the  CONSOLE  icon  and  size  the  CONSOLE  window  as  desired. 

Step  3  .  Press  enter  to  get  to  the  command  prompt. 

Step  4;  Use  the  change  directory  command  (cd)  to  change  to  the  directory  shown. 
WorkSpace/thesis/arsim 

Step  5:  Check  to  ensure  you  are  in  the  correct  directory  using  the  present  work  directory 
command  (pwd).  The  correct  directory  is  as  shown. 

/n/auvsim4Avork/aquarobo/WorkSpace/thesis/arsim 

Step  6;  You  have  now  completed  the  Login  process  and  are  positioned  in  the  correct 
working  directory. 


n.  COMPILING 

Step  7:  To  compile  this  version  program,  type  in  the  make  command  {make)  and  press 
enter.  The  make  command  has  executed  properly  when  the  command  prompt  returns. 
Once  the  command  prompt  returns,  the  program  has  compiled  correctly. 

m.  USING  THE  SIMULATOR 

Step  8;  At  the  command  prompt,  type  aqua  to  start  the  AquaRobot  Simulator.  Some 
interaction  between  the  user  and  the  simulator  is  now  required.  When  prompted, 
determine  whether  you  want  to  see  the  discrete  (type  in  0)  or  continuous  (type  in  1) 
alternating  tripod  gaits.  Then  determine  the  desired  goal  point  and  enter  its  x-coordinate, 
space,  and  y-coordinate.  The  positive  x-axis  lies  lengthwise  to  the  right  on  the  screen  and 
the  positive  y-axis  is  outward  towards  the  viewer.  Next,  you  may  be  prompted  to  select 
the  footpad  size  for  the  simulation.  Choosing  zero  (0)  selects  the  25  centimeter  pootpad; 
choosing  one  (1)  selects  the  45  centimeter  footpad.  The  CONSOLE  window  shows 
program  status  and  the  AQUAROBOT  window  shows  the  graphic  output. 
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Step  9;  Four  viewpoints  are  available  to  the  user  as  a  right  mouse  button  menu;  Camera: 
ABOVE,  Camera;  LEG5-6  VIEW,  Camera;  LEGl  VIEW,  and  Camera;  LEG4  VIEW.  To 
change  the  viewpoint  from  the  default  Camera:  LEG2-3  VIEW  view,  quickly  press  the 
right  mouse  button  afrer  typing  aqua  and  moving  the  mouse  pointer  into  the 
AQUAROBOT  window. 

NOTE;  The  menu  selections  FBLEREAD,  RESETFILE,  and  KEYBDREAD 
should  not  be  used. 

Step  10;  The  RESET  menu  selection  places  AquaRobot  in  the  default  LEG2-3 
VIEW. 

Step  11:  To  repeat  the  aqua  program,  with  the  default  Camera.  LEG2-3  VIEW 

viewpoint,  move  the  mouse  pointer  into  the  CONSOLE  window  and  choose  one  of  the 
following  methods; 

a.  type  aqua 

b.  press  enter 

type  a  (this  is  the  history/repeat  sequence) 
press  enter 

Step  12:  To  exit  the  aqua  program,  press  the  right  mouse  menu  button  and  select 

Exit  from  the  choices  listed. 

Step  13:  To  exit  the  CONSOLE  window,  use  the  right  mouse  button  to  get  Log 

Out;  then  answer  "yes". 
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ATTACHMENT  A:  FLOWCHART  and  FUNCTION  DESCRIPTIONS 
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The  simulator  files  are  separated  into  four  modules.  The  files  that  comprise  the 
Makefile,  Graphics,  and  Matrix  Manipulation  Modules  are  only  cursorily  addressed  here. 
The  Gait  Planning  Module  is  described  in  more  detail. 

1.  Makefile 

The  Makefile  allows  the  make  utility  to  intelligently  compile  the  program.  This 
file  includes  instructions  for  how  and  when  to  compile  the  va'^'ous  files  that  comprise  the 
AquaRobot  Simulator.  The  AquaRobot  simulation  program  uses  the  UNIX  make  utility  to 
link  together  the  27  individual  files  with  the  graphics,  math,  and  standard  input-output 
libraries.  In  this  case,  the  Makefile  generates  the  executable  program  aqua. 

2.  Graphics 

The  3D  stick-figure  graphics  code  was  originally  written  by  Sandra  Davidson 
(pAV93].  The  graphics  files  (code  modules)  are  included  in  Appendix  C  for  easier 
reference.  Some  file  names  were  changed  to  avoid  software  configuration  management 
problems.  Additionally,  some  graphics  code  was  modified  because  of  the  requirements  of 
the  gait  planning  code.  For  example,  the  FARCLIPPING  value  and  the  CAMERA  viewing 
angles  were  changed  in  the  bot.H  file  and  the  CB  height  was  changed  in  the  AbBody.C 
file.  Because  the  Graphics  Module  polls  the  Gait  Planning  Module,  a  suitable  interface 
between  the  two  was  constructed  in  the  Kmematics.H  and  Kine,matics.C  files.  The 
graphics  code  consists  of  the  following  files  . 

♦  AbBody.H  and  AbBocfy.  C; 

♦  A  bLeg.H  and  AbLeg.  C, 

♦  AbRigidH  and  AbRigid  C; 

♦  bot.H  and  bot.C; 

♦  Kmematics.H  and  Kinematics.  C; 
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♦  Link.H  and  Link.  C, 

♦  LinkO.H  and  LinkO.  C; 

♦  Linkl.H  and  Linkl.  C\ 

♦  Link2.H  and  Link2.  C,  and 

♦  Links. H  and  Links.  C. 

3.  Matrix  Manipulation 

The  matrix  manipulation  code  was  also  originally  written  by  Sandra  Davidson 
[DAV93].  It  provides  4x4  matrix  multiplication  capability.  This  code  is  an  essential  part 
of  the  graphics  code,  although  it  is  not  used  in  the  gait  planning  code.  The  Matrix 
Manipulation  Module  includes  the  following  files,  found  in  Appendix  C: 

♦  Matrixkfy.H  and  MatrixMy.  C. 

4.  Gait  Planning 

The  Gait  Planning  Module  (GPM)  consists  of  the  following  files; 

♦  arconslH; 

♦  arfunc.H  and  atfunc.  C;  and 

♦  artpgaitH  and  artpgait.  C. 

To  support  easier  reference  and  maintenance,  most  constants  are  grouped  together  into 
the  file  arconst.H  (aquarobot  constants). 

The  arfunc.H/C  (aquarobot  functions)  files  contain  13  functions.  The  min  and 
max  functions  simply  return  the  minimum  or  maximum  of  two  expressions,  respectively. 
The  ellipse  function  calculates  the  incremental  foot  trajectory  along  an  elliptical  path 
between  the  current  foot  position  and  the  desired  foot  position.  The  kinematics  function 
performs  kinematics  for  the  Gait  Planning  Module.  Kinematics  for  the  Graphics  Module 
are  performed  in  the  Graphics  Kinematics.C/H  files.  The  inv  kinematics  function 
performs  the  inverse  kinematics  operations.  The  world  bocfy  function  provides  coordinate 
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transformation  from  the  body-fixed  coordinate  system  to  the  world  coordinate  system. 
The  bocfy_\vorld function  provides  coordinate  transformation  from  the  world  coordinate 
system  to  the  body-fixed  coordinate  system. 

The  maxdistance_25cm  function  determines  the  maximum  stride  for  the 
workspace  defined  by  the  25  centimeter  footpad.  The  maxdistance_45cm  function 
determines  the  maximum  stride  for  the  workspace  defined  by  the  45  centuneter  footpad. 
The  arcint  function  calculates  the  intersection  of  a  directed  line  and  an  arc  segment.  The 
segint  function  calculates  the  intersection  of  a  directed  line  and  a  line  segment.  The  stable 
function  calculates  the  stability  of  a  tripod.  The  staparam  function  determines  the 
longitudinal  stability  margin  and  the  x  and  y  intercepts  of  the  directional  ray  originating  at 
the  CB  and  terminating  at  the  point  of  intersection  on  the  selected  tripod. 

The  artpgait.H/C  (aquarobot  tripod  gait)  files  15  functions.  The  get_goal 
function  allows  the  user  to  input  the  desired  goal  point  to  which  the  AquaRobot  simulation 
walks.  The  getjnem  function  alh  vs  the  user  to  choose  between  the  DATG  or  the 
CATG.  The  get Jbotchoice  function  allows  the  user  to  determine  whether  the  25 
centimeter  or  45  centimeter  footpad  is  used.  The  gait  algorithm  function  executes  either 
the  DATG  or  the  CATG  gait  algorithms,  as  previously  determined  by  the  user.  The 
robot jnodel  function  initializes  the  joint  angles  for  the  simulation. 

The  disc  set  Jlag  function  initializei>  the  flags  used  during  the  DATG  algorithm. 
The  disc^ait planning  function  determines  the  walking  parameters,  such  as  stride  length 
and  direction,  used  during  the  DATG  algorithm.  The  discjripodjnotion  function 
calculates  and  executes  incremental  joint  motions  necessary  to  move  the  AquaRobot 
simulation  using  the  DATG. 

The  cont  set  Jlag  function  initializes  the  flags  used  during  the  CATG  algorithm. 
The  cont^ait  jplanning  function  determines  the  walking  parameters,  such  as  stride  length 
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and  direction,  used  during  the  CATG  algorithm.  The  contjripod  motion  function 
calculates  and  executes  the  incremental  joint  motions  necessary  to  move  the  AquaRobot 
simulation  using  the  CATG. 

Some  functions  in  the  artpgaitC  file  are  used  only  for  printing  out  the  status  of 
joints,  feet,  etc.  These  functions  include  print  status,  print  walkpara,  print _gnu_data, 
KnA  print Jointdata. 
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ATTACHMENT  B:  REQUIRED  FH^ES  LISTING 


Makefile 

Makefile 

Graphics 

AbBodyH 

AbBody.C 

AbLegH 

AbLeg.C 

AbRigidH 

AbRigid.C 

bot.H 

bot.C 

Kinematics.H 

Kinematics.C 

Link.H 

Link.C 

LinkOH 

LinkO.C 

LinkLH 

Linkl.C 

Link2.H 

Link2.C 

Links  H 

Links. C 

Matrix  Manipulation 

Matrb^y.C 

MatrixMy.H 

Gait  Planning 
arconst.H 
arfunc.H 
arfunc.C 
artpgait.H 
artpgait.C 
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